diff --git a/src/humanoid_3d_animation/humanoid_simulation.py b/src/humanoid_3d_animation/humanoid_simulation.py index 350a6e0290..77602e8375 100644 --- a/src/humanoid_3d_animation/humanoid_simulation.py +++ b/src/humanoid_3d_animation/humanoid_simulation.py @@ -7,98 +7,151 @@ import signal import sys import random -from dataclasses import dataclass +from dataclasses import dataclass, field import numpy as np import matplotlib.pyplot as plt from matplotlib.animation import FuncAnimation +from typing import Dict, List, Optional, Tuple +import logging + +# ====================== 日志配置 ====================== +logging.basicConfig( + level=logging.INFO, + format="%(asctime)s - %(levelname)s - %(message)s", + handlers=[ + logging.StreamHandler(sys.stdout) + ] +) +logger = logging.getLogger(__name__) + +# ====================== 全局配置 ====================== +# 全局运行状态 +sim_running = True +# 线程安全锁 +data_lock = threading.Lock() -# ====================== 配置抽离 ====================== +# ====================== 配置类（增强版） ====================== @dataclass class SimConfig: """仿真配置类：集中管理所有可配置参数""" - # 仿真参数 + # 仿真核心参数 timestep: float = 0.005 sim_frequency: float = 2.0 state_print_interval: float = 1.0 + # 相机参数 cam_distance: float = 2.0 cam_azimuth: float = 45.0 cam_elevation: float = -20.0 - # 关节运动幅度配置（针对不同动作优化） - joint_amplitudes = { + + # 关节运动幅度配置 + joint_amplitudes: Dict[str, float] = field(default_factory=lambda: { "left_shoulder": 1.2, "right_shoulder": 1.2, "left_elbow": 1.0, "right_elbow": 1.0, "left_hip": 1.0, "right_hip": 1.0, "left_knee": 1.2, "right_knee": 1.2 - } - # 控制模式（新增行走和挥手动作） + }) + + # 控制模式 default_mode: str = "walk" + supported_modes: List[str] = field(default_factory=lambda: ["walk", "wave", "sin", "random", "stop"]) + # 可视化配置 - plot_update_interval: int = 50 # 绘图更新间隔（帧数） - max_plot_points: int = 200 # 图表最大显示数据点 - # 动作参数 - walk_stride: float = 0.8 # 行走步幅 - wave_frequency: float = 1.5 # 挥手频率 + plot_update_interval: int = 50 + max_plot_points: int = 200 + plot_refresh_ms: int = 50 + # 动作参数 + walk_stride: float = 0.8 + wave_frequency: float = 1.5 + smooth_factor: float = 0.05 # 控制信号平滑因子 -# 全局变量 -sim_running = True -# 用于线程间数据共享的锁 -data_lock = threading.Lock() + # 性能配置 + max_fps: int = 60 # 最大帧率限制 + step_sleep: float = 0.001 # 步长休眠时间 -def signal_handler(sig, frame): - """处理Ctrl+C中断信号""" +# ====================== 信号处理 ====================== +def signal_handler(sig: int, frame) -> None: + """优雅处理中断信号""" global sim_running sim_running = False - print("\n⚠️ 收到中断信号，正在退出仿真...") + logger.warning("收到中断信号，正在优雅退出仿真...") signal.signal(signal.SIGINT, signal_handler) -# ====================== 核心功能类 ====================== +# ====================== 核心仿真类 ====================== class HumanoidSimulator: def __init__(self, config: SimConfig): self.config = config - self.model = None - self.data = None - self.joint_names = list(config.joint_amplitudes.keys()) - self.joint_ctrl_ids = {} - self.joint_qpos_indices = {} - self.current_mode = config.default_mode - self.last_ctrl_signals = {} - - # 新增：动作状态变量 - self.walk_phase = 0.0 # 行走相位 - self.wave_arm = "right" # 当前挥动手臂 - - # 可视化相关变量 - self.plot_data = {name: [] for name in self.joint_names} - self.time_data = [] - self.frame_counter = 0 - - # 绘图相关 - self.fig, self.ax = None, None - self.lines = {} - self.ani = None - - def load_model(self): - """加载MuJoCo模型（完全修复XML格式）""" - # 完全兼容所有MuJoCo版本的XML - xml_content = """ - -