diff --git a/src/humanoid_3d_animation/humanoid_simulation.py b/src/humanoid_3d_animation/humanoid_simulation.py index d1e74460a1..4eb868e559 100644 --- a/src/humanoid_3d_animation/humanoid_simulation.py +++ b/src/humanoid_3d_animation/humanoid_simulation.py @@ -6,19 +6,23 @@ import threading import signal import sys -import select # 新增:用于非阻塞输入监听 -from dataclasses import dataclass # 用于配置类 +import select +from dataclasses import dataclass +import numpy as np +import matplotlib.pyplot as plt +from matplotlib.animation import FuncAnimation -# ====================== 配置抽离(核心优化点)====================== + +# ====================== 配置抽离 ====================== @dataclass class SimConfig: """仿真配置类:集中管理所有可配置参数""" - # 文件路径配置(XML文件在当前脚本所在目录) + # 文件路径配置 xml_filename: str = "humanoid.xml" # 仿真参数 - timestep: float = 0.005 # 与XML中的timestep保持一致 - sim_frequency: float = 2.0 # 关节运动频率(Hz) - state_print_interval: float = 1.0 # 状态打印间隔(秒) + 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 @@ -30,22 +34,29 @@ class SimConfig: "left_hip": 0.8, "right_hip": 0.8, "left_knee": 0.6, "right_knee": 0.6 } - # 控制模式:sin(正弦运动)、random(随机运动)、stop(静止) + # 控制模式 default_mode: str = "sin" + # 可视化配置 + plot_update_interval: int = 50 # 绘图更新间隔(帧数) + max_plot_points: int = 200 # 图表最大显示数据点 + -# 全局变量:用于优雅退出 +# 全局变量 sim_running = True +# 用于线程间数据共享的锁 +data_lock = threading.Lock() + def signal_handler(sig, frame): - """处理Ctrl+C中断信号,实现优雅退出""" + """处理Ctrl+C中断信号""" global sim_running sim_running = False print("\n⚠️ 收到中断信号,正在退出仿真...") - # 这里不再直接sys.exit,让主循环自然退出 -# 注册信号处理 + signal.signal(signal.SIGINT, signal_handler) + # ====================== 核心功能类 ====================== class HumanoidSimulator: def __init__(self, config: SimConfig): @@ -53,15 +64,22 @@ def __init__(self, config: SimConfig): self.model = None self.data = None self.joint_names = list(config.joint_amplitudes.keys()) - # 预存关节ID和控制ID(避免每次循环重复计算,性能优化) self.joint_ctrl_ids = {} self.joint_qpos_indices = {} - # 运动模式和控制信号缓存(用于平滑控制) self.current_mode = config.default_mode - self.last_ctrl_signals = {} # 存储上一帧的控制信号 - # 新增:线程停止标记(用于优雅停止输入监听线程) + self.last_ctrl_signals = {} self.input_thread_running = False + # 新增:可视化相关变量 + 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 create_xml_file(self, file_path): """创建人形机器人XML文件""" xml_content = f""" @@ -146,18 +164,15 @@ def create_xml_file(self, file_path): print(f"✅ 已在 {file_path} 创建XML文件!") def load_model(self): - """加载MuJoCo模型,预存关节ID和控制ID(性能优化)""" - # 核心修改:获取当前脚本所在的文件夹路径 + """加载MuJoCo模型""" current_dir = os.path.dirname(os.path.abspath(__file__)) self.model_path = os.path.join(current_dir, self.config.xml_filename) - # 检查并创建文件 if not os.path.exists(self.model_path): self.create_xml_file(self.model_path) else: print(f"ℹ️ XML文件已存在(路径:{self.model_path}),无需重新创建!") - # 读取XML内容并加载模型 try: with open(self.model_path, "r", encoding="utf-8") as f: xml_content = f.read() @@ -168,31 +183,27 @@ def load_model(self): print(f"❌ 模型加载失败:{e}") sys.exit(1) - # 预存关节控制ID和qpos索引(只计算一次,性能优化) + # 预存关节ID for name in self.joint_names: - # 获取控制ID ctrl_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"{name}_motor") if ctrl_id == -1: ctrl_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, name) self.joint_ctrl_ids[name] = ctrl_id - # 获取qpos索引(根关节占前7个自由度) joint_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, name) if joint_id != -1: self.joint_qpos_indices[name] = 7 + joint_id else: self.joint_qpos_indices[name] = -1 - # 初始化控制信号缓存 self.last_ctrl_signals[name] = 0.0 def get_joint_ctrl_signal(self, name, t): - """根据运动模式生成关节控制信号(功能扩展:多模式)""" + """生成关节控制信号""" amplitude = self.config.joint_amplitudes[name] freq = self.config.sim_frequency if self.current_mode == "sin": - # 正弦/余弦运动:左右关节反向 if "left" in name or "hip" in name or "knee" in name: if "shoulder" in name or "elbow" in name: signal = math.sin(t * freq) * amplitude @@ -204,80 +215,143 @@ def get_joint_ctrl_signal(self, name, t): else: signal = -math.cos(t * freq) * amplitude elif self.current_mode == "random": - # 随机运动:在幅度范围内随机变化 signal = (math.sin(t * freq * 0.5) * 0.5 + 0.5) * amplitude * 2 - amplitude elif self.current_mode == "stop": - # 静止:控制信号为0 signal = 0.0 else: signal = 0.0 - # 平滑过渡:避免控制信号突变(用户体验优化) - smooth_factor = 0.1 # 平滑系数,越小越平滑 + # 平滑过渡 + smooth_factor = 0.1 self.last_ctrl_signals[name] = (1 - smooth_factor) * self.last_ctrl_signals[name] + smooth_factor * signal return self.last_ctrl_signals[name] def update_joint_controls(self): - """更新关节控制信号(函数拆分:主循环更简洁)""" + """更新关节控制信号""" t = self.data.time for name in self.joint_names: ctrl_id = self.joint_ctrl_ids[name] if ctrl_id == -1: continue - # 生成控制信号并设置 ctrl_signal = self.get_joint_ctrl_signal(name, t) try: self.data.ctrl[ctrl_id] = ctrl_signal except Exception as e: print(f"⚠️ 关节 {name} 控制失败:{e}") + def collect_plot_data(self): + """收集绘图数据(线程安全)""" + self.frame_counter += 1 + if self.frame_counter % self.config.plot_update_interval != 0: + return + + with data_lock: + # 添加时间数据 + current_time = self.data.time + self.time_data.append(current_time) + + # 添加各关节角度数据 + for name in self.joint_names: + qpos_idx = self.joint_qpos_indices[name] + if qpos_idx != -1 and qpos_idx < len(self.data.qpos): + angle = self.data.qpos[qpos_idx] + self.plot_data[name].append(angle) + + # 限制数据点数量,避免内存占用过大 + if len(self.time_data) > self.config.max_plot_points: + self.time_data.pop(0) + for name in self.joint_names: + if len(self.plot_data[name]) > 0: + self.plot_data[name].pop(0) + + def init_plot(self): + """初始化绘图界面""" + plt.style.use('seaborn-v0_8-darkgrid') + self.fig, self.ax = plt.subplots(figsize=(12, 8)) + self.ax.set_xlabel('Time (s)', fontsize=12) + self.ax.set_ylabel('Joint Angle (rad)', fontsize=12) + self.ax.set_title('Real-time Joint Angle Monitoring', fontsize=14, fontweight='bold') + + # 定义颜色方案 + colors = ['#FF6B6B', '#4ECDC4', '#45B7D1', '#96CEB4', '#FECA57', '#FF9FF3', '#54A0FF', '#5F27CD'] + linestyles = ['-', '--', '-.', ':', '-', '--', '-.', ':'] + + # 创建线条对象 + for i, name in enumerate(self.joint_names): + line, = self.ax.plot([], [], label=name, color=colors[i % len(colors)], + linestyle=linestyles[i % len(linestyles)], linewidth=2) + self.lines[name] = line + + self.ax.legend(loc='upper right', fontsize=10) + self.ax.grid(True, alpha=0.3) + + # 设置y轴范围 + self.ax.set_ylim(-2, 2) + + plt.tight_layout() + print("📊 关节角度可视化图表已创建!") + + def update_plot(self, frame): + """更新绘图(动画回调函数)""" + with data_lock: + # 更新每条线的数据 + for name, line in self.lines.items(): + if len(self.plot_data[name]) > 0 and len(self.time_data) == len(self.plot_data[name]): + line.set_data(self.time_data, self.plot_data[name]) + + # 自动调整x轴范围 + if len(self.time_data) > 0: + self.ax.set_xlim(max(0, self.time_data[-1] - 10), self.time_data[-1] + 1) + + return list(self.lines.values()) + def print_robot_state(self): - """打印机器人状态(优化:控制打印频率,添加帧率显示)""" + """打印机器人状态""" current_time = self.data.time if not hasattr(self, "last_print_time"): self.last_print_time = 0.0 self.frame_count = 0 self.start_time = current_time - # 累计帧数,计算帧率 self.frame_count += 1 elapsed_time = current_time - self.start_time if elapsed_time > 0: self.fps = self.frame_count / elapsed_time - # 按间隔打印 if current_time - self.last_print_time >= self.config.state_print_interval: print(f"\n===== 机器人状态(时间:{current_time:.2f}s | 帧率:{self.fps:.1f} FPS)=====") for name in self.joint_names: ctrl_id = self.joint_ctrl_ids[name] qpos_idx = self.joint_qpos_indices[name] if ctrl_id != -1 and qpos_idx != -1 and qpos_idx < len(self.data.qpos): - print(f"关节 {name}: 位置 = {self.data.qpos[qpos_idx]:.2f} rad, 控制信号 = {self.data.ctrl[ctrl_id]:.2f}") + print( + f"关节 {name}: 位置 = {self.data.qpos[qpos_idx]:.2f} rad, 控制信号 = {self.data.ctrl[ctrl_id]:.2f}") self.last_print_time = current_time def reset_robot(self): """重置机器人到初始状态""" - mujoco.mj_resetData(self.model, self.data) - self.data.qpos[0:7] = [0, 0, 1.0, 1, 0, 0, 0] - # 重置控制信号缓存 - for name in self.joint_names: - self.last_ctrl_signals[name] = 0.0 + with data_lock: + mujoco.mj_resetData(self.model, self.data) + self.data.qpos[0:7] = [0, 0, 1.0, 1, 0, 0, 0] + # 重置控制信号缓存 + for name in self.joint_names: + self.last_ctrl_signals[name] = 0.0 + # 清空绘图数据 + self.plot_data = {name: [] for name in self.joint_names} + self.time_data = [] + self.frame_counter = 0 print("\n🔄 机器人已重置到初始状态!") def input_listener(self): - """后台线程:非阻塞监听控制台输入(修复锁冲突问题)""" + """后台线程:监听控制台输入""" global sim_running - # 设置线程运行标记 self.input_thread_running = True - # 非阻塞读取的超时时间(避免线程空转) timeout = 0.1 while self.input_thread_running and sim_running: try: - # 使用select检查是否有输入(非阻塞) ready, _, _ = select.select([sys.stdin], [], [], timeout) if ready: - # 读取输入(此时有数据,不会阻塞) user_input = sys.stdin.readline().strip().lower() if user_input == 'r': self.reset_robot() @@ -287,28 +361,43 @@ def input_listener(self): elif user_input == 'q': sim_running = False print("\n📤 收到退出指令,仿真将结束...") - elif user_input: # 非空的未知指令 - print(f"\n❓ 未知指令:{user_input},支持的指令:r(重置)、sin/random/stop(模式)、q(退出)") + elif user_input == 'clear': + with data_lock: + self.plot_data = {name: [] for name in self.joint_names} + self.time_data = [] + print("\n🧹 绘图数据已清空!") + elif user_input: + print(f"\n❓ 未知指令:{user_input},支持的指令:") + print(" - r:重置机器人") + print(" - sin/random/stop:切换运动模式") + print(" - clear:清空绘图数据") + print(" - q:退出仿真") except Exception as e: - # 捕获异常,避免线程崩溃 print(f"\n⚠️ 输入处理失败:{e}") break - # 线程退出提示(可选) print("\n🔌 输入监听线程已优雅退出") def run_simulation(self): """运行仿真主循环""" - # 加载模型 self.load_model() - # 启动输入监听线程(不再设置daemon=True,靠标记控制退出) + # 初始化绘图 + self.init_plot() + + # 启动输入监听线程 input_thread = threading.Thread(target=self.input_listener) input_thread.start() - # 启动可视化 + # 启动可视化动画 + self.ani = FuncAnimation(self.fig, self.update_plot, interval=50, blit=True, cache_frame_data=False) + + # 显示绘图窗口(非阻塞) + plt.show(block=False) + + # 启动MuJoCo可视化 with viewer.launch_passive(self.model, self.data) as v: - # 设置相机参数(配置化) + # 设置相机参数 pelvis_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, "pelvis") if pelvis_id != -1: v.cam.trackbodyid = pelvis_id @@ -316,25 +405,26 @@ def run_simulation(self): v.cam.azimuth = self.config.cam_azimuth v.cam.elevation = self.config.cam_elevation - # 打印操作提示(用户体验优化) + # 打印操作提示 print("\n📌 仿真操作提示:") print(" - 输入 'r' 回车:重置机器人") print(" - 输入 'sin'/'random'/'stop' 回车:切换运动模式") + print(" - 输入 'clear' 回车:清空绘图数据") print(" - 输入 'q' 回车:退出仿真") print(" - 按 Ctrl+C:强制退出仿真") print("\n🚀 仿真开始...") - # 仿真主循环(使用perf_counter优化时间控制) + # 仿真主循环 global sim_running last_step_time = time.perf_counter() + while sim_running and v.is_running(): - # 控制仿真步长(更精准的时间控制) current_time = time.perf_counter() if current_time - last_step_time >= self.config.timestep: # 更新关节控制 self.update_joint_controls() - # 执行仿真步(异常捕获,健壮性优化) + # 执行仿真步 try: mujoco.mj_step(self.model, self.data) except Exception as e: @@ -344,24 +434,39 @@ def run_simulation(self): # 更新可视化 v.sync() + # 收集绘图数据 + self.collect_plot_data() + # 打印状态 self.print_robot_state() last_step_time = current_time - # 优雅停止输入监听线程 + # 处理matplotlib事件 + plt.pause(0.001) + + # 停止输入监听线程 self.input_thread_running = False - # 等待线程退出(最多等待1秒,避免卡死) input_thread.join(timeout=1.0) + # 关闭绘图窗口 + plt.close(self.fig) + print("\n🏁 仿真结束!") + # ====================== 程序入口 ====================== if __name__ == "__main__": + # 设置matplotlib后端(避免显示问题) + import matplotlib + + matplotlib.use('TkAgg') + # 初始化配置 config = SimConfig() + # 创建仿真器并运行 simulator = HumanoidSimulator(config) simulator.run_simulation() - # 程序退出前清理(可选) + sys.exit(0) \ No newline at end of file