From 7b5d2cf132329cec7991da0bc541e00e905e9a86 Mon Sep 17 00:00:00 2001 From: Weiyu1105 <86831759+Weiyu1105@users.noreply.github.com> Date: Mon, 4 Aug 2025 21:51:57 +0800 Subject: [PATCH] =?UTF-8?q?feat:=20=E7=B5=B1=E4=B8=80=E8=A7=80=E5=AF=9F?= =?UTF-8?q?=E5=B7=A5=E5=85=B7=E8=88=87=E7=AD=96=E7=95=A5=E6=8E=A7=E5=88=B6?= =?UTF-8?q?=20dispatch\n\n-=20=E5=BB=BA=E7=AB=8B=20calculate=5Frelative=5F?= =?UTF-8?q?joint=5Fpositions=20=E4=BE=9B=E6=A8=A1=E6=93=AC=E8=88=87?= =?UTF-8?q?=E7=A1=AC=E9=AB=94=E5=85=B1=E7=94=A8\n-=20Promote=20default=5Fp?= =?UTF-8?q?ose=20to=20config=20and=20verify=20against=20XML\n-=20Refactor?= =?UTF-8?q?=20controllers=20with=20dispatch=20table=20for=20clearer?= =?UTF-8?q?=E6=A8=A1=E5=BC=8F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- config.py | 5 +- config.yaml | 1 + hardware_controller.py | 203 +++++++++++++++----------------- main.py | 18 +-- main_nicegui.py | 2 +- observation.py | 9 +- policy.py | 231 ++++++++++++------------------------- rendering.py | 25 ++-- simulation.py | 9 +- simulation_controller.py | 39 +++++-- state.py | 5 +- test/verify_model_mode.py | 3 +- utils/__init__.py | 1 + utils/observation_utils.py | 8 ++ 14 files changed, 252 insertions(+), 307 deletions(-) create mode 100644 utils/__init__.py create mode 100644 utils/observation_utils.py diff --git a/config.py b/config.py index ad67eda..f682582 100644 --- a/config.py +++ b/config.py @@ -39,7 +39,7 @@ class AppConfig: keyboard_velocity_adjust_step: float gamepad_sensitivity: Dict[str, float] param_adjust_steps: Dict[str, float] - + default_pose: List[float] # 【新增】預設站姿 initial_tuning_params: TuningParamsConfig floating_controller: FloatingControllerConfig @@ -74,7 +74,8 @@ def load_config(path: str = "config.yaml") -> AppConfig: keyboard_velocity_adjust_step=config_data['keyboard_velocity_adjust_step'], gamepad_sensitivity=config_data['gamepad_sensitivity'], param_adjust_steps=config_data['param_adjust_steps'], - + + default_pose=config_data['default_pose'], # 新增欄位 initial_tuning_params=tuning_params, floating_controller=floating_config ) diff --git a/config.yaml b/config.yaml index 49345e4..0d02c38 100644 --- a/config.yaml +++ b/config.yaml @@ -67,6 +67,7 @@ policy_transition_duration: 0.5 # 平滑過渡的持續時間 (秒) # 核心參數 (Core Parameters) num_motors: 12 +default_pose: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # 【新增】預設站姿 physics_timestep: 0.004 # <-- 建議修改：提高模擬精度以增強穩定性 control_freq: 50.0 warmup_duration: 0.0 diff --git a/hardware_controller.py b/hardware_controller.py index 9158f54..ccf10a6 100644 --- a/hardware_controller.py +++ b/hardware_controller.py @@ -8,6 +8,7 @@ import numpy as np from scipy.spatial.transform import Rotation from typing import TYPE_CHECKING +from utils.observation_utils import calculate_relative_joint_positions if TYPE_CHECKING: from config import AppConfig @@ -19,44 +20,47 @@ class RobotStateHardware: """【修改】只儲存AI決策所需的、單位正確的數據。""" def __init__(self): # 這些是直接從Teensy的POLICY_STREAM解析來的數據 - self.angular_velocity_radps = np.zeros(3, dtype=np.float32) - self.gravity_vector_norm = np.zeros(3, dtype=np.float32) - self.accelerometer_ms2 = np.zeros(3, dtype=np.float32) - self.pitch_rad = 0.0 - self.joint_positions_rad = np.zeros(12, dtype=np.float32) - self.joint_velocities_radps = np.zeros(12, dtype=np.float32) - + self.angular_velocity_radps = np.zeros(3, dtype=np.float32) # 角速度 (rad/s) + self.gravity_vector_norm = np.zeros(3, dtype=np.float32) # 標準化的重力向量 + self.accelerometer_ms2 = np.zeros(3, dtype=np.float32) # 加速度 (m/s^2) + self.pitch_rad = 0.0 # 俯仰角 (rad) + self.joint_positions_rad = np.zeros(12, dtype=np.float32) # 關節絕對角度 (rad) + self.joint_velocities_radps = np.zeros(12, dtype=np.float32) # 關節角速度 (rad/s) + # 這些是PC端自己維護的狀態 - self.last_action = np.zeros(12, dtype=np.float32) - self.command = np.zeros(3, dtype=np.float32) - - self.last_update_time = 0.0 + self.last_action = np.zeros(12, dtype=np.float32) # 上一幀的AI動作輸出 + self.command = np.zeros(3, dtype=np.float32) # 使用者指令 + + self.last_update_time = 0.0 # 上次收到Teensy數據的時間戳 class HardwareController: """【修改版】管理與實體硬體的AI控制迴圈，從SerialCommunicator借用連接。""" - + def __init__(self, config: 'AppConfig', policy: 'PolicyManager', global_state: 'SimulationState', serial_comm: 'SerialCommunicator'): """【修改】初始化時接收 SerialCommunicator 的參考。""" self.config = config self.policy = policy self.global_state = global_state - self.serial_comm = serial_comm - - self.ser = None - self.is_running = False - self.read_thread = None - self.control_thread = None - - self.hw_state = RobotStateHardware() - self.lock = threading.Lock() - self.ai_control_enabled = threading.Event() - - self.foot_positions_in_body = np.array([ - [-0.0804, -0.1759, -0.1964], - [ 0.0806, -0.1759, -0.1964], - [-0.0804, 0.0239, -0.1964], - [ 0.0806, 0.0239, -0.1964], - ], dtype=np.float32) + self.serial_comm = serial_comm # 保存序列通訊器的參考 + + self.ser = None # 序列埠物件 + self.is_running = False # 執行緒運行旗標 + self.read_thread = None # 讀取執行緒 + self.control_thread = None # 控制執行緒 + + self.hw_state = RobotStateHardware() # 實體機器人狀態物件 + self.lock = threading.Lock() # 保護 hw_state 的執行緒鎖 + self.ai_control_enabled = threading.Event() # 用於啟用/暫停AI控制的事件 + + # 【修改】從設定檔取得預設站姿，避免依賴模擬器 + self.default_pose = np.array(self.config.default_pose, dtype=np.float32) + + # 控制模式分派表，使用策略模式減少 if-else + self.control_dispatch = { + "JOINT_TEST": self._cmd_joint_test, + "HARDWARE_MODE": self._cmd_hardware_mode, + } + log.info("✅ 硬體控制器已初始化。") def start_controller_threads(self): @@ -84,13 +88,13 @@ def start_controller_threads(self): # 自動切換 Teensy 到 AI 決策流模式 try: log.info(" -> 正在命令 Teensy 切換至 POLICY_STREAM 模式...") - self.ser.write(b"monitor p\n") - time.sleep(0.1) # 給Teensy一點時間切換模式並清空緩衝區 + self.ser.write(b"monitor p\n") # 發送 'monitor p' 指令 + time.sleep(0.1) # 給Teensy一點時間切換模式並清空緩衝區 self.ser.reset_input_buffer() log.info(" -> Teensy 模式切換指令已發送。") except serial.SerialException as e: log.error(f"❌ 發送模式切換指令失敗: {e}") - self.stop_controller_threads() # 使用現有的停止函式來清理 + self.stop_controller_threads() return # 後續的執行緒啟動邏輯 @@ -110,14 +114,14 @@ def start_controller_threads(self): def stop_controller_threads(self): """【修改】在停止執行緒後，自動命令Teensy恢復到安全的人類友好模式。""" - if not self.is_running: return - + if not self.is_running: + return + log.info("正在停止硬體控制器...") self.is_running = False self.disable_ai() - self.ai_control_enabled.set() - - # 在交還控制權前，命令Teensy恢復安全狀態 + self.ai_control_enabled.set() # 釋放可能在等待的控制執行緒 + if self.ser and self.ser.is_open: try: log.info(" -> 正在命令 Teensy 停止運動並恢復 HUMAN 遙測模式...") @@ -128,20 +132,20 @@ def stop_controller_threads(self): except serial.SerialException: log.warning(" -> 警告: 發送停止指令失敗，可能連接已斷開。") - # 所有執行緒清理和交還控制權的邏輯 if self.control_thread and self.control_thread.is_alive(): self.control_thread.join(timeout=1) if self.read_thread and self.read_thread.is_alive(): self.read_thread.join(timeout=1) - + if self.serial_comm: self.serial_comm.is_managed_by_hardware_controller = False log.info("序列埠控制權已交還。") - + self.ser = None log.info("硬體控制器已完全停止。") - + def enable_ai(self): + """啟用AI控制。""" if not self.is_running: log.info("無法啟用 AI：硬體控制器未運行。") return @@ -151,25 +155,23 @@ def enable_ai(self): self.global_state.hardware_ai_is_active = True def disable_ai(self): + """禁用AI控制，並發送停止指令給機器人。""" log.info("⏸️ AI 控制已暫停。") self.ai_control_enabled.clear() self.global_state.hardware_ai_is_active = False - if self.is_running and self.ser and self.ser.is_open: # 增加 is_running 判斷 - try: self.ser.write(b"stop\n") + if self.is_running and self.ser and self.ser.is_open: + try: + self.ser.write(b"stop\n") except serial.SerialException as e: log.error(f"發送停止指令失敗: {e}") def parse_policy_stream(self, line: str): - """ - 專門解析來自 Teensy 'monitor p' 模式的 34 維數據流。 - """ + """專門解析來自 Teensy 'monitor p' 模式的 34 維數據流。""" try: parts = line.split(',') if len(parts) != 34: return - data_vec = np.array(parts, dtype=np.float32) - with self.lock: self.hw_state.angular_velocity_radps[:] = data_vec[0:3] self.hw_state.gravity_vector_norm[:] = data_vec[3:6] @@ -178,43 +180,39 @@ def parse_policy_stream(self, line: str): self.hw_state.joint_positions_rad[:] = data_vec[10:22] self.hw_state.joint_velocities_radps[:] = data_vec[22:34] self.hw_state.last_update_time = time.time() - except (ValueError, IndexError) as e: log.error(f"❌ 解析 POLICY_STREAM 時出錯: {e} | 原始數據長度: {len(parts)}") def construct_observation(self) -> np.ndarray: """ - [全新重構] 從 hw_state 中直接獲取數據，並拼接成最終的 ONNX 輸入向量。 - 不再需要任何客戶端的估算。 + [全新重構] 從 hw_state 中獲取數據，並拼接成最終的 ONNX 輸入向量。 + 【核心修復】此處修正了關節角度和線性速度的問題。 """ with self.lock: - # 【保留】從全域狀態獲取用戶指令 self.hw_state.command = self.global_state.command * np.array(self.config.command_scaling_factors) - - # 【修改】建立一個全新的、數據源清晰的字典 + + # 【核心修復 #1】使用工具函式計算相對關節角度 + relative_joint_positions = calculate_relative_joint_positions( + self.hw_state.joint_positions_rad, self.default_pose + ) + obs_list = { - # --- 來自Teensy的高品質數據 --- 'angular_velocity': self.hw_state.angular_velocity_radps, 'gravity_vector': self.hw_state.gravity_vector_norm, 'accelerometer': self.hw_state.accelerometer_ms2, - 'pitch': np.array([self.hw_state.pitch_rad]), # 確保是1維向量 - 'joint_positions': self.hw_state.joint_positions_rad, + 'pitch': np.array([self.hw_state.pitch_rad]), + 'joint_positions': relative_joint_positions, 'joint_velocities': self.hw_state.joint_velocities_radps, - - # --- PC端自行維護的狀態 --- 'last_action': self.hw_state.last_action, 'commands': self.hw_state.command, - - # --- 為了兼容性，保留舊的鍵，用零填充 --- - 'linear_velocity': np.zeros(3), + # 【核心修復 #2】硬體無法提供線速度，用零填充 + 'linear_velocity': np.zeros(3), } - - # 【保留】由配方驅動的拼接邏輯 + recipe = self.policy.get_active_recipe() if not recipe: log.warning("⚠️ 警告: 無法從策略管理器獲取有效的觀察配方。") return np.array([]) - try: final_obs_list = [obs_list[key] for key in recipe] return np.concatenate(final_obs_list).astype(np.float32) @@ -223,72 +221,61 @@ def construct_observation(self) -> np.ndarray: return np.array([]) def _read_from_port(self): + """背景執行緒：從序列埠讀取數據。""" log.info("[硬體讀取線程已啟動] 等待來自 Teensy 的 POLICY_STREAM 數據...") while self.is_running: if not self.ser or not self.ser.is_open: - # 【修改】確保呼叫正確的停止函式 self.stop_controller_threads() break try: line = self.ser.readline().decode('utf-8', errors='ignore').strip() if line: - # 【修改】呼叫新的解析器 - self.parse_policy_stream(line) + self.parse_policy_stream(line) except (serial.SerialException, OSError): log.error("❌ 錯誤：序列埠斷開連接或讀取錯誤。") self.stop_controller_threads() break except Exception as e: log.error(f"❌ _read_from_port 發生未知錯誤: {e}") - + def _control_loop(self): - """【保留大部分邏輯】此迴圈現在是硬體指令的唯一來源，並能感知 JOINT_TEST 模式。""" + """背景執行緒：根據模式生成並發送控制指令。""" log.info("\n--- 硬體控制執行緒已就緒 ---") - # 【保留】 default_pose_hardware 保持不變 - default_pose_hardware = self.global_state.sim.default_pose + while self.is_running: loop_start_time = time.perf_counter() - command_to_send = None - - # 【保留】 JOINT_TEST 模式的指令生成邏輯不變 - if self.global_state.control_mode == "JOINT_TEST": - final_command = default_pose_hardware + self.global_state.joint_test_offsets - action_str = ' '.join(f"{a:.4f}" for a in final_command) - command_to_send = f"move all {action_str}\n" - - # 【保留】 HARDWARE_MODE 的指令生成邏輯 - elif self.global_state.control_mode == "HARDWARE_MODE": - # 【保留】等待AI啟用信號 - self.ai_control_enabled.wait() - if not self.is_running: break - - # 【保留】呼叫新的 construct_observation 函式 - # 這個函式現在會使用 RobotStateHardware 中更新過的高品質數據 - observation = self.construct_observation() - if observation.size == 0: - time.sleep(0.02); continue - - # 【保留】運行AI策略獲取動作 - _, action_raw = self.policy.get_action_for_hardware(observation) - with self.lock: - # 【保留】更新 last_action - self.hw_state.last_action[:] = action_raw - - # 【保留】生成發送給Teensy的文字指令 - final_command = default_pose_hardware + action_raw * self.global_state.tuning_params.action_scale - action_str = ' '.join(f"{a:.4f}" for a in final_command) - command_to_send = f"move all {action_str}\n" - - # 【保留】發送指令邏輯 + handler = self.control_dispatch.get(self.global_state.control_mode) + command_to_send = handler() if handler else None + if command_to_send and self.ser and self.ser.is_open: try: self.ser.write(command_to_send.encode('utf-8')) except serial.SerialException: - # 【修改】確保呼叫一致的停止函式 - self.stop_controller_threads() # <--- 這裡從 self.stop() 改為 self.stop_controller_threads() - - # 【保留】迴圈時間管理 + self.stop_controller_threads() + loop_duration = time.perf_counter() - loop_start_time sleep_time = (1.0 / self.config.control_freq) - loop_duration if sleep_time > 0: time.sleep(sleep_time) + + def _cmd_joint_test(self) -> str | None: + """關節手動測試模式的指令。""" + final_command = self.default_pose + self.global_state.joint_test_offsets + action_str = ' '.join(f"{a:.4f}" for a in final_command) + return f"move all {action_str}\n" + + def _cmd_hardware_mode(self) -> str | None: + """硬體 AI 模式的指令。""" + self.ai_control_enabled.wait() + if not self.is_running: + return None + observation = self.construct_observation() + if observation.size == 0: + time.sleep(0.02) + return None + _, action_raw = self.policy.get_action_for_hardware(observation) + with self.lock: + self.hw_state.last_action[:] = action_raw + final_command = self.default_pose + action_raw * self.global_state.tuning_params.action_scale + action_str = ' '.join(f"{a:.4f}" for a in final_command) + return f"move all {action_str}\n" diff --git a/main.py b/main.py index 6bb0377..29c7f43 100644 --- a/main.py +++ b/main.py @@ -42,7 +42,7 @@ def main(): xbox_handler = XboxInputHandler(state) - obs_builder = ObservationBuilder(sim.data, sim.model, sim.torso_id, sim.default_pose, config) + obs_builder = ObservationBuilder(sim.data, sim.model, sim.torso_id, config) # 在無 GUI 版本中仍建立 DebugOverlay 以顯示文字資訊 overlay = DebugOverlay() @@ -133,19 +133,11 @@ def soft_reset(): terrain_manager.update(state.latest_pos, state.terrain_mode) if state.control_mode == "HARDWARE_MODE": - if hw_controller.is_running: - with hw_controller.lock: - t_since_update = time.time() - hw_controller.hw_state.last_update_time - conn_status = f"Data Delay: {t_since_update:.2f}s" if t_since_update < 1.0 else "Data Timeout!" - state.hardware_status_text = f"Connection Status: {conn_status}\n" - state.hardware_status_text += f"LinVel: {np.array2string(hw_controller.hw_state.lin_vel_local, precision=2)}\n" - state.hardware_status_text += f"Gyro: {np.array2string(hw_controller.hw_state.imu_gyro_radps, precision=2)}" - else: - state.hardware_status_text = "Hardware controller not running." - + # 硬體模式下，模擬器僅負責渲染，控制邏輯在硬體控制器執行緒中 + pass elif state.control_mode == "SERIAL_MODE": pass - else: # 模擬模式 (WALKING, FLOATING, etc.) + else: # 模擬模式 (WALKING, FLOATING, etc.) if state.single_step_mode: print("\n" + "="*20 + f" STEP AT TIME {sim.data.time:.4f} " + "="*20) onnx_input, action_final = policy_manager.get_action(state.command) @@ -171,7 +163,7 @@ def soft_reset(): sim.render(state) # --- 7. 程式結束，清理資源 --- - hw_controller.stop() + hw_controller.stop_controller_threads() sim.close() xbox_handler.close() serial_comm.close() diff --git a/main_nicegui.py b/main_nicegui.py index 03400b8..444eacf 100644 --- a/main_nicegui.py +++ b/main_nicegui.py @@ -28,7 +28,7 @@ def create_simulation_components(use_sim: bool, config): sim = Simulation(config) terrain = TerrainManager(sim.model, sim.data) floating = FloatingController(config, sim.model, sim.data, terrain) - obs = ObservationBuilder(sim.data, sim.model, sim.torso_id, sim.default_pose, config) + obs = ObservationBuilder(sim.data, sim.model, sim.torso_id, config) return sim, obs, terrain, floating else: log.info("🚫 Simulation disabled, using mock components.") diff --git a/observation.py b/observation.py index e69b9c0..a3d8c51 100644 --- a/observation.py +++ b/observation.py @@ -2,17 +2,19 @@ import numpy as np import mujoco from typing import TYPE_CHECKING, List, Dict +from utils.observation_utils import calculate_relative_joint_positions if TYPE_CHECKING: from config import AppConfig class ObservationBuilder: - def __init__(self, data, model, torso_id, default_pose, config: 'AppConfig'): + def __init__(self, data, model, torso_id, config: 'AppConfig'): self.recipe = [] # 初始化為空，將由外部設定 self.data = data self.model = model self.torso_id = torso_id - self.default_pose = default_pose + # 【新增】從設定檔獲得預設站姿 + self.default_pose = np.array(config.default_pose, dtype=np.float32) self.config = config try: @@ -101,7 +103,8 @@ def _get_commands(self, command, **kwargs): return command * np.array(self.config.command_scaling_factors) def _get_joint_positions(self, **kwargs): - return self.data.qpos[7:] - self.default_pose + # 使用共用工具函式計算相對關節角度 + return calculate_relative_joint_positions(self.data.qpos[7:], self.default_pose) def _get_joint_velocities(self, **kwargs): return self.data.qvel[6:].copy() diff --git a/policy.py b/policy.py index a3be771..accbb51 100644 --- a/policy.py +++ b/policy.py @@ -5,7 +5,8 @@ import os import time from collections import deque -from typing import TYPE_CHECKING, List, Dict +from typing import TYPE_CHECKING, List, Dict, Tuple +from logger import log # 為了型別提示，避免循環匯入 if TYPE_CHECKING: @@ -15,98 +16,75 @@ class PolicyManager: """ - 【版本 2.0】 - AI 策略大腦 + 【版本 2.1 - 重構版】 - AI 策略大腦 管理多個 ONNX 策略模型。它能夠： 1. 在啟動時載入所有在 config.yaml 中定義的模型。 2. 為每個模型維護獨立的觀察歷史，以支援需要多幀輸入的模型。 3. 根據使用者指令，在兩個不同的策略模型之間進行平滑的線性融合（插值）。 - 4. 提供獨立的介面供模擬 (`get_action`) 和實體硬體 (`get_action_for_hardware`) 使用。 + 4. 【重構】合併了模擬和硬體的動作獲取邏輯，減少程式碼重複。 """ def __init__(self, config: 'AppConfig', obs_builder: 'ObservationBuilder', overlay: 'DebugOverlay'): - self.config = config # 儲存應用程式的全域設定 - self.obs_builder = obs_builder # 儲存觀察向量產生器的參考 - self.overlay = overlay # 儲存除錯介面(DebugOverlay)的參考 - self.sessions: Dict[str, ort.InferenceSession] = {} # 字典，儲存已載入的 ONNX 推論 session，鍵為模型名稱 - self.model_recipes: Dict[str, List[str]] = {} # 字典，儲存每個模型對應的觀察配方 - self.model_history_lengths: Dict[str, int] = {} # 字典，儲存每個模型需要的歷史觀察幀數 - self.model_names: List[str] = [] # 列表，儲存所有成功載入模型的名稱 - + self.config = config # 儲存應用程式的全域設定 + self.obs_builder = obs_builder # 儲存觀察向量產生器的參考 + self.overlay = overlay # 除錯介面(DebugOverlay)的參考 + self.sessions: Dict[str, ort.InferenceSession] = {} # 以模型名稱為鍵的 session 字典 + self.model_recipes: Dict[str, List[str]] = {} # 每個模型的觀察配方 + self.model_history_lengths: Dict[str, int] = {} # 每個模型需要的歷史幀數 + self.model_names: List[str] = [] + print("--- 正在載入所有 ONNX 模型及其配方 ---") - # 遍歷設定檔中定義的所有模型 for name, model_info in config.onnx_models.items(): - path = model_info.get('path') # 獲取模型檔案路徑 - recipe = model_info.get('observation_recipe') # 獲取模型對應的觀察配方 - - # 如果模型資訊不完整，則跳過 + path = model_info.get('path') + recipe = model_info.get('observation_recipe') if not path or not recipe: - print(f" ⚠️ 警告: 模型 '{name}' 缺少 'path' 或 'observation_recipe'，已跳過。") + log.warning(f"模型 '{name}' 缺少 'path' 或 'observation_recipe'，已跳過。") continue - print(f" - 載入模型 '{name}' 從: {path}") + log.info(f" - 載入模型 '{name}' 從: {path}") try: - # --- ONNX Runtime 優化與載入 --- - sess_options = ort.SessionOptions() # 建立 ONNX Runtime 的 session 設定 - # 定義優化後模型的快取檔案路徑，例如 "model.onnx" -> "model.optimized.ort" + sess_options = ort.SessionOptions() cache_path = os.path.splitext(path)[0] + ".optimized.ort" - sess_options.optimized_model_filepath = cache_path # 設定快取路徑 - sess_options.graph_optimization_level = ort.GraphOptimizationLevel.ORT_ENABLE_ALL # 啟用所有圖優化 - - # 載入 session。如果 .ort 快取檔案已存在且最新，會直接載入；否則會進行優化並生成快取檔 + sess_options.optimized_model_filepath = cache_path + sess_options.graph_optimization_level = ort.GraphOptimizationLevel.ORT_ENABLE_ALL session = ort.InferenceSession(path, sess_options=sess_options, providers=['CPUExecutionProvider']) - # --- 推斷模型輸入維度和歷史長度 --- - # 暫時設定配方以計算單幀觀察的維度 + # 推斷觀察維度與歷史長度 self.obs_builder.set_recipe(recipe) base_obs_dim = len(self.obs_builder.get_observation(np.zeros(3), np.zeros(config.num_motors))) - - # 從模型本身獲取其輸入層的總維度 model_input_dim = session.get_inputs()[0].shape[1] - - # 計算模型需要的歷史幀數 (history length) - history_len = 1 # 預設為1 + history_len = 1 if base_obs_dim > 0 and model_input_dim % base_obs_dim == 0: history_len = model_input_dim // base_obs_dim - - # 儲存該模型的所有相關資訊 + self.sessions[name] = session self.model_recipes[name] = recipe self.model_history_lengths[name] = history_len self.model_names.append(name) - print(f" > 配方: {recipe}") - print(f" > 基礎維度: {base_obs_dim}, 模型輸入: {model_input_dim}, 推斷歷史長度: {history_len}") - + log.info(f" > 配方: {recipe}") + log.info(f" > 基礎維度: {base_obs_dim}, 模型輸入: {model_input_dim}, 推斷歷史長度: {history_len}") except Exception as e: - print(f" ❌ 錯誤: 無法載入模型 '{name}'。錯誤: {e}") + log.error(f"無法載入模型 '{name}'。錯誤: {e}") - # 如果沒有任何模型成功載入，則終止程式 if not self.sessions: sys.exit("❌ 致命錯誤: 未能成功載入任何 ONNX 模型。") - # --- 狀態變數，用於管理多模型融合 --- - self.primary_policy_name = self.model_names[0] # 當前穩定的主要策略，預設為第一個載入的模型 - self.source_policy_name = self.model_names[0] # 開始轉換時的來源策略 - self.target_policy_name = self.model_names[0] # 正在轉換去的目標策略 - - self.last_action = np.zeros(config.num_motors, dtype=np.float32) # 初始化上一次的動作向量 - - # 為每個模型維護一個獨立的觀察歷史佇列 + self.primary_policy_name = self.model_names[0] + self.source_policy_name = self.model_names[0] + self.target_policy_name = self.model_names[0] + self.last_action = np.zeros(config.num_motors, dtype=np.float32) self.obs_histories: Dict[str, deque] = {} - - self.is_transitioning = False # 是否正在進行模型融合的旗標 - self.transition_start_time = 0.0 # 融合開始的時間戳 - self.transition_alpha = 0.0 # 線性融合的權重 (0.0=source, 1.0=target) + self.is_transitioning = False + self.transition_start_time = 0.0 + self.transition_alpha = 0.0 - self.reset() # 初始化所有模型的觀察歷史 + self.reset() print("--- 正在預熱所有 ONNX 模型 (強制進行首次推論優化)... ---") - # 遍歷所有載入的 session for name, session in self.sessions.items(): - input_name = session.get_inputs()[0].name # 獲取輸入層名稱 - output_name = session.get_outputs()[0].name # 獲取輸出層名稱 - model_input_dim = session.get_inputs()[0].shape[1] # 獲取輸入維度 - dummy_input = np.zeros((1, model_input_dim), dtype=np.float32) # 建立一個符合維度的假輸入 + input_name = session.get_inputs()[0].name + output_name = session.get_outputs()[0].name + dummy_input = np.zeros((1, session.get_inputs()[0].shape[1]), dtype=np.float32) try: - # 執行一次推論以觸發可能的 JIT 編譯或優化 session.run([output_name], {input_name: dummy_input}) print(f" - 模型 '{name}' 預熱成功。") except Exception as e: @@ -115,150 +93,93 @@ def __init__(self, config: 'AppConfig', obs_builder: 'ObservationBuilder', overl print(f"✅ 策略管理器初始化完成，主要模型: '{self.primary_policy_name}'") def get_active_recipe(self) -> List[str]: - """一個輔助函式，返回當前主要策略所使用的觀察配方，主要供 HardwareController 使用。""" + """返回目前主要模型所需的觀察配方。""" return self.model_recipes.get(self.primary_policy_name, []) def select_target_policy(self, target_name: str): - """(由鍵盤觸發) 選擇一個目標策略並開始平滑轉換。""" - # 檢查目標模型是否存在 + """(由UI或鍵盤觸發) 選擇一個目標策略並開始平滑轉換。""" if target_name not in self.sessions: - print(f"⚠️ 警告: 無法切換，目標模型 '{target_name}' 不存在。") + log.warning(f"無法切換，目標模型 '{target_name}' 不存在。") return - # 如果正在轉換中，或目標就是當前的主要模型，則不執行任何操作 if self.is_transitioning or target_name == self.primary_policy_name: return - - print(f"🚀 開始從 '{self.primary_policy_name}' 線性融合至 '{target_name}'...") - self.is_transitioning = True # 設定轉換旗標 - self.transition_start_time = time.time() # 記錄起始時間 - self.transition_alpha = 0.0 # 重置融合權重 - self.source_policy_name = self.primary_policy_name # 當前的主要模型成為來源 - self.target_policy_name = target_name # 設定目標模型 - - def get_action(self, command: np.ndarray) -> tuple[np.ndarray, np.ndarray]: - """ - 【模擬專用】獲取最終動作。此版本會自己建立觀察向量，運行所有模型，並根據狀態進行融合。 - """ - all_actions = {} # 建立一個字典來儲存本幀所有模型的輸出 - primary_onnx_input = np.array([]) # 用於除錯顯示的輸入 - - # --- 步驟 1: 運行所有模型，獲取各自的輸出 --- - for name, session in self.sessions.items(): - recipe = self.model_recipes[name] - self.obs_builder.set_recipe(recipe) # 動態設定觀察產生器的配方 - - # 產生觀察並更新對應模型的歷史 - base_obs = self.obs_builder.get_observation(command, self.last_action) - self.obs_histories[name].append(base_obs) - - # 將歷史觀察拼接成 ONNX 的最終輸入 - onnx_input = np.concatenate(list(self.obs_histories[name])).astype(np.float32).reshape(1, -1) - - # 檢查維度是否匹配，以防萬一 - if onnx_input.shape[1] != session.get_inputs()[0].shape[1]: - action_raw = np.zeros(self.config.num_motors, dtype=np.float32) - else: - input_name = session.get_inputs()[0].name - output_name = session.get_outputs()[0].name - action_raw = session.run([output_name], {input_name: onnx_input})[0].flatten() # 執行推論 - - all_actions[name] = action_raw # 將模型的輸出存入字典 - - # 如果是當前主要模型，儲存其輸入以供除錯介面顯示 - if name == self.primary_policy_name: - primary_onnx_input = onnx_input - - # --- 步驟 2: 根據狀態決定最終動作 --- - if self.is_transitioning: - elapsed = time.time() - self.transition_start_time # 計算經過時間 - duration = self.config.policy_transition_duration # 讀取設定的總時長 - - # 線性計算 alpha，並限制在 [0, 1] 範圍 - if duration > 0: self.transition_alpha = min(elapsed / duration, 1.0) - else: self.transition_alpha = 1.0 - - # 根據 alpha 在來源和目標策略的輸出之間進行線性插值 (Lerp) - source_action = all_actions[self.source_policy_name] - target_action = all_actions[self.target_policy_name] - final_action = (1.0 - self.transition_alpha) * source_action + self.transition_alpha * target_action - - # 如果融合完成 - if self.transition_alpha >= 1.0: - print(f"✅ 已完成向 '{self.target_policy_name}' 的融合。") - self.is_transitioning = False # 結束轉換狀態 - self.primary_policy_name = self.target_policy_name # 目標模型成為新的主要模型 - else: - # 如果不在轉換中，直接使用主要模型的輸出 - final_action = all_actions[self.primary_policy_name] - - self.last_action[:] = final_action # 更新 last_action 供下一幀使用 - return primary_onnx_input, final_action # 返回主要模型的輸入和最終融合後的動作 - - def get_action_for_hardware(self, observation: np.ndarray) -> tuple[np.ndarray, np.ndarray]: + log.info(f"🚀 開始從 '{self.primary_policy_name}' 融合至 '{target_name}'...") + self.is_transitioning = True + self.transition_start_time = time.time() + self.transition_alpha = 0.0 + self.source_policy_name = self.primary_policy_name + self.target_policy_name = target_name + + def _get_action_internal(self, base_obs: np.ndarray) -> Tuple[np.ndarray, np.ndarray]: """ - 【硬體專用】獲取最終動作。此版本接收一個已經由 HardwareController 建立好的觀察向量。 + 【重構】內部核心函式：處理觀察歷史並運行所有模型，回傳主要模型輸入與最終動作。 """ - all_actions = {} + all_actions: Dict[str, np.ndarray] = {} primary_onnx_input = np.array([]) for name, session in self.sessions.items(): - # 硬體模式下，觀察向量已經建立好，我們只需根據歷史長度要求來維護歷史 - self.obs_histories[name].append(observation) - + self.obs_histories[name].append(base_obs) onnx_input = np.concatenate(list(self.obs_histories[name])).astype(np.float32).reshape(1, -1) - if onnx_input.shape[1] != session.get_inputs()[0].shape[1]: action_raw = np.zeros(self.config.num_motors, dtype=np.float32) else: input_name = session.get_inputs()[0].name output_name = session.get_outputs()[0].name action_raw = session.run([output_name], {input_name: onnx_input})[0].flatten() - all_actions[name] = action_raw - if name == self.primary_policy_name: primary_onnx_input = onnx_input - # 融合邏輯與 get_action 完全相同 if self.is_transitioning: elapsed = time.time() - self.transition_start_time duration = self.config.policy_transition_duration - if duration > 0: self.transition_alpha = min(elapsed / duration, 1.0) - else: self.transition_alpha = 1.0 + self.transition_alpha = min(elapsed / duration, 1.0) if duration > 0 else 1.0 source_action = all_actions[self.source_policy_name] target_action = all_actions[self.target_policy_name] final_action = (1.0 - self.transition_alpha) * source_action + self.transition_alpha * target_action if self.transition_alpha >= 1.0: + log.info(f"✅ 已完成向 '{self.target_policy_name}' 的融合。") self.is_transitioning = False self.primary_policy_name = self.target_policy_name else: final_action = all_actions[self.primary_policy_name] - self.last_action[:] = final_action # last_action 也需要為硬體模式更新 + self.last_action[:] = final_action return primary_onnx_input, final_action + def get_action(self, command: np.ndarray) -> Tuple[np.ndarray, np.ndarray]: + """ + 【模擬專用】此函式會自行利用 ObservationBuilder 來生成單幀觀察向量。 + """ + self.obs_builder.set_recipe(self.get_active_recipe()) + base_obs = self.obs_builder.get_observation(command, self.last_action) + return self._get_action_internal(base_obs) + + def get_action_for_hardware(self, observation: np.ndarray) -> Tuple[np.ndarray, np.ndarray]: + """ + 【硬體專用】接收已由 HardwareController 建立好的觀察向量。 + 注意：硬體端無法提供 `linear_velocity`，若模型需要此資訊仍會以零填充並發出警告。 + """ + active_recipe = self.get_active_recipe() + if 'linear_velocity' in active_recipe: + log.warning("警告：目前策略需要 'linear_velocity'，但硬體不支援此數據。模型表現可能不佳。") + return self._get_action_internal(observation) + def reset(self): - """重置所有模型的觀察歷史，並同步更新除錯介面。""" - # 重置主模型的觀察配方，用於UI顯示 + """重置所有模型的觀察歷史與相關狀態。""" active_recipe = self.model_recipes[self.primary_policy_name] self.obs_builder.set_recipe(active_recipe) if self.overlay: self.overlay.set_recipe(active_recipe) - - # 為每個模型初始化獨立的、填滿零的觀察歷史佇列 for name in self.model_names: recipe = self.model_recipes[name] - self.obs_builder.set_recipe(recipe) # 臨時設定以計算維度 + self.obs_builder.set_recipe(recipe) base_obs_dim = len(self.obs_builder.get_observation(np.zeros(3), np.zeros(self.config.num_motors))) history_length = self.model_history_lengths[name] - - # 建立一個固定長度的雙向佇列 (deque)，當新元素加入時，舊元素會自動被擠出 self.obs_histories[name] = deque( [np.zeros(base_obs_dim, dtype=np.float32)] * history_length, - maxlen=history_length + maxlen=history_length, ) - - # 恢復 obs_builder 為主要模型的配方，以供模擬使用 self.obs_builder.set_recipe(active_recipe) - self.is_transitioning = False # 強制停止任何正在進行的轉換 - print(f"✅ 所有策略狀態已重置。主要模型: '{self.primary_policy_name}'。") + self.is_transitioning = False + log.info(f"所有策略狀態已重置。主要模型: '{self.primary_policy_name}'。") diff --git a/rendering.py b/rendering.py index d18652c..5fea872 100644 --- a/rendering.py +++ b/rendering.py @@ -73,7 +73,7 @@ def render(self, viewport, context, state: SimulationState, sim: "Simulation"): self.render_simulation_overlay(viewport, context, state, sim) def render_hardware_overlay(self, viewport, context, state: SimulationState): - """渲染硬體控制模式的專用介面，使用 MjrRect 進行精確排版。""" + """【修復】渲染硬體控制模式的專用介面，顯示正確的感測器數據。""" padding = 10 panel_width = int(viewport.width * 0.45) panel_height = int(viewport.height * 0.6) @@ -95,20 +95,29 @@ def render_hardware_overlay(self, viewport, context, state: SimulationState): else: policy_text = f"Active Policy: {pm.primary_policy_name}" - status_text = f"--- Real-time Hardware Status ---\n{state.hardware_status_text}" sensor_text = "" hw_ctrl = state.hardware_controller_ref if hw_ctrl and hw_ctrl.is_running: with hw_ctrl.lock: - imu_acc_str = np.array2string(hw_ctrl.hw_state.imu_acc_g, precision=2, suppress_small=True) - joint_pos_str = np.array2string(hw_ctrl.hw_state.joint_positions_rad, precision=2, suppress_small=True, max_line_width=80) + t_since_update = time.time() - hw_ctrl.hw_state.last_update_time + conn_status = f"Data Delay: {t_since_update:.2f}s" if t_since_update < 1.0 else "Data Timeout!" + + acc_str = np.array2string(hw_ctrl.hw_state.accelerometer_ms2, precision=2) + gyro_str = np.array2string(hw_ctrl.hw_state.angular_velocity_radps, precision=2) + joint_pos_str = np.array2string(hw_ctrl.hw_state.joint_positions_rad, precision=2, max_line_width=80) + sensor_text = ( - f"\n\n--- Sensor Readings (from Robot) ---\n" - f"IMU Acc (g): {imu_acc_str}\n" + f"--- Real-time Hardware Status ---\n" + f"Connection: {conn_status}\n\n" + f"--- Sensor Readings (from Robot) ---\n" + f"Accelerometer (m/s^2): {acc_str}\n" + f"Gyro (rad/s): {gyro_str}\n" f"Joint Pos (rad):\n{joint_pos_str}" ) - - full_text = f"{title}\n\n{help_text}\n\n{policy_text}\n\n{status_text}{sensor_text}" + else: + sensor_text = "Hardware controller not running." + + full_text = f"{title}\n\n{help_text}\n\n{policy_text}\n\n{sensor_text}" mujoco.mjr_overlay(mujoco.mjtFont.mjFONT_NORMAL, mujoco.mjtGridPos.mjGRID_TOPLEFT, top_left_rect, full_text, " ", context) cmd_panel_height = int(viewport.height * 0.1) diff --git a/simulation.py b/simulation.py index 560683b..3ed5223 100644 --- a/simulation.py +++ b/simulation.py @@ -47,11 +47,14 @@ def __init__(self, config: AppConfig): sys.exit("❌ 錯誤: 在 XML 中找不到名為 'torso' 的 body。") home_key_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_KEY, 'home') + xml_pose = None if home_key_id != -1: - self.default_pose = self.model.key_qpos[home_key_id][7:].copy() + xml_pose = self.model.key_qpos[home_key_id][7:].copy() else: - self.default_pose = np.zeros(config.num_motors) - print("⚠️ 警告: 在 XML 中未找到名為 'home' 的 keyframe，將使用零作為預設姿態。") + print("⚠️ 警告: 在 XML 中未找到名為 'home' 的 keyframe。") + self.default_pose = np.array(config.default_pose, dtype=np.float32) + if xml_pose is not None and not np.allclose(xml_pose, self.default_pose, atol=1e-4): + print("⚠️ 警告: config.yaml 的 default_pose 與 XML 中的 'home' 姿態不一致。以設定檔為主。") # 視窗與渲染上下文將在模擬執行緒中初始化 diff --git a/simulation_controller.py b/simulation_controller.py index 793b273..ae0d9b1 100644 --- a/simulation_controller.py +++ b/simulation_controller.py @@ -39,6 +39,14 @@ def __init__(self, state: SimulationState) -> None: # 追蹤手動模式下懸浮是否已啟用 self._manual_float_active = False + # 控制模式分派表，策略模式 + self.control_dispatch = { + "WALKING": self._ctrl_from_ai, + "FLOATING": self._ctrl_from_ai, + "JOINT_TEST": self._ctrl_from_joint_test, + "MANUAL_CTRL": self._ctrl_from_manual, + } + # 初始化將在執行緒啟動後進行 # ------------------------------------------------------------------ @@ -208,16 +216,8 @@ def _simulation_step(self) -> None: control_mode = self.state.control_mode tuning_params = self.state.tuning_params - onnx_input, action_final = self.policy_manager.get_action(command) - - if control_mode == "MANUAL_CTRL": - with self.state.lock: - final_ctrl = self.state.manual_final_ctrl.copy() - elif control_mode == "JOINT_TEST": - with self.state.lock: - final_ctrl = self.sim.default_pose + self.state.joint_test_offsets - else: - final_ctrl = self.sim.default_pose + action_final * tuning_params.action_scale + handler = self.control_dispatch.get(control_mode, self._ctrl_from_ai) + final_ctrl, onnx_input, action_final = handler(command, tuning_params) self.sim.apply_position_control(final_ctrl, tuning_params) @@ -232,6 +232,25 @@ def _simulation_step(self) -> None: break mujoco.mj_step(self.sim.model, self.sim.data) + def _ctrl_from_ai(self, command, tuning_params): + """AI 控制模式處理函式。""" + onnx_input, action_final = self.policy_manager.get_action(command) + final_ctrl = self.sim.default_pose + action_final * tuning_params.action_scale + return final_ctrl, onnx_input, action_final + + def _ctrl_from_joint_test(self, command, tuning_params): + """關節測試模式處理函式。""" + with self.state.lock: + offsets = self.state.joint_test_offsets.copy() + final_ctrl = self.sim.default_pose + offsets + return final_ctrl, np.array([]), np.zeros(self.config.num_motors) + + def _ctrl_from_manual(self, command, tuning_params): + """手動控制模式處理函式。""" + with self.state.lock: + manual_ctrl = self.state.manual_final_ctrl.copy() + return manual_ctrl, np.array([]), np.zeros(self.config.num_motors) + # ------------------------------------------------------------------ def stop(self) -> None: self._running.clear() diff --git a/state.py b/state.py index 39837ba..e14566f 100644 --- a/state.py +++ b/state.py @@ -87,7 +87,6 @@ class SimulationState: hardware_is_connected: bool = False # 標記硬體控制器是否已成功啟動 hardware_ai_is_active: bool = False # 標記硬體模式下的 AI 是否已啟用 - hardware_status_text: str = "Not Connected" # 用於在 UI 上顯示的硬體狀態文字 single_step_mode: bool = False # 標記是否處於單步模擬模式 execute_one_step: bool = False # 在單步模式下，請求執行下一步的旗標 @@ -141,8 +140,8 @@ def set_control_mode(self, new_mode: str): if new_mode == "JOINT_TEST": self.joint_test_offsets.fill(0.0) elif new_mode == "MANUAL_CTRL": - initial_pose = self.sim.default_pose.copy() if hasattr(self.sim, 'default_pose') else np.zeros(self.config.num_motors) - self.manual_final_ctrl[:] = initial_pose + # 直接從設定檔載入預設站姿 + self.manual_final_ctrl[:] = np.array(self.config.default_pose) # 若從手動模式回到 AI 模式，重置 AI 狀態 is_entering_ai = new_mode in ["WALKING", "FLOATING"] diff --git a/test/verify_model_mode.py b/test/verify_model_mode.py index 6d08cf9..4034fd5 100644 --- a/test/verify_model_mode.py +++ b/test/verify_model_mode.py @@ -99,7 +99,8 @@ def verify(): recipe = config.observation_recipes[obs_dim] # 【核心】我們在這裡實例化的 obs_builder 會使用您修改後的 absolute mode 版本 - obs_builder = ObservationBuilder(recipe, data, model, mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, 'torso'), default_pose_from_key, config) + obs_builder = ObservationBuilder(data, model, mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, 'torso'), config) + obs_builder.set_recipe(recipe) base_obs_dim = len(obs_builder.get_observation(np.zeros(3), np.zeros(config.num_motors))) policy_config = config diff --git a/utils/__init__.py b/utils/__init__.py new file mode 100644 index 0000000..db3e327 --- /dev/null +++ b/utils/__init__.py @@ -0,0 +1 @@ +# utils package diff --git a/utils/observation_utils.py b/utils/observation_utils.py new file mode 100644 index 0000000..d5f211a --- /dev/null +++ b/utils/observation_utils.py @@ -0,0 +1,8 @@ +import numpy as np + + +def calculate_relative_joint_positions(absolute_positions: np.ndarray, default_pose: np.ndarray) -> np.ndarray: + """計算相對關節角度。 + Compute joint angles relative to default pose. + """ + return absolute_positions - default_pose