From 4c438f25bdf0c52399594bd0eaab65bf8eaecf09 Mon Sep 17 00:00:00 2001 From: oyzw2302 Date: Sat, 20 Dec 2025 17:16:07 +0800 Subject: [PATCH 01/12] =?UTF-8?q?=E6=96=B0=E5=88=9B=E5=BB=BA=E4=B8=80?= =?UTF-8?q?=E4=B8=AA=E4=BB=BF=E7=9C=9F=E4=BA=BA=E6=A8=A1=E5=9E=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../humanoid_simulation.py | 138 ++++++++++++++++++ 1 file changed, 138 insertions(+) create mode 100644 src/humanoid_3d_animation/humanoid_simulation.py diff --git a/src/humanoid_3d_animation/humanoid_simulation.py b/src/humanoid_3d_animation/humanoid_simulation.py new file mode 100644 index 0000000000..68cdeedf1b --- /dev/null +++ b/src/humanoid_3d_animation/humanoid_simulation.py @@ -0,0 +1,138 @@ +import mujoco +import mujoco.viewer as viewer +import os +import time + +def create_humanoid_xml(file_path): + """自动创建humanoid.xml文件并写入模型代码""" + xml_content = """ + + """ + with open(file_path, "w", encoding="utf-8") as f: + f.write(xml_content) + print(f"已自动在 {file_path} 创建humanoid.xml文件！") + +def run_humanoid_simulation(): + # 直接写死桌面路径（替换成你的实际用户名，这里保留你的用户名） + # 注意：路径中的反斜杠用双反斜杠，或单反斜杠加r前缀 + model_path = r"C:\Users\欧阳志威1\Desktop\humanoid.xml" # r前缀表示原始字符串，避免转义 + + # 打印路径 + print(f"===== 模型文件路径 =====") + print(f"模型文件完整路径：{model_path}") + print(f"========================") + + # 检查并创建文件 + if not os.path.exists(model_path): + create_humanoid_xml(model_path) + else: + print("humanoid.xml文件已存在，无需重新创建！") + + # 加载模型（新增：用Python内置的open函数测试是否能读取文件） + try: + # 先测试Python是否能读取该文件（排除系统权限问题） + with open(model_path, "r", encoding="utf-8") as f: + f.read() + print("Python内置函数已成功读取文件，权限正常！") + except Exception as e: + print(f"Python读取文件失败，权限/路径问题：{e}") + return + + # 加载MuJoCo模型 + try: + model = mujoco.MjModel.from_xml_path(model_path) + data = mujoco.MjData(model) + print("模型加载成功！开始启动仿真...") + except Exception as e: + print(f"MuJoCo加载模型失败：{e}") + # 备选方案：从字符串加载模型（绕过文件读取） + print("尝试从字符串直接加载模型...") + with open(model_path, "r", encoding="utf-8") as f: + xml_str = f.read() + model = mujoco.MjModel.from_xml_string(xml_str) + data = mujoco.MjData(model) + print("从字符串加载模型成功！开始启动仿真...") + + # 运行仿真 + with viewer.launch_passive(model, data) as v: + sim_steps = 10000 + step_interval = 0.01 + print("仿真开始，按窗口关闭按钮退出...") + for _ in range(sim_steps): + mujoco.mj_step(model, data) + v.sync() + time.sleep(step_interval) + print("仿真结束！") + +if __name__ == "__main__": + run_humanoid_simulation() \ No newline at end of file From a5107672b37afda5da2477f61786b9534f0e190a Mon Sep 17 00:00:00 2001 From: oyzw2302 Date: Sat, 20 Dec 2025 22:08:17 +0800 Subject: [PATCH 02/12] =?UTF-8?q?=E9=A1=B6=E9=83=A8=E5=AF=BC=E5=85=A5?= =?UTF-8?q?=E5=8C=BA=E6=96=B0=E5=A2=9E=E6=95=B0=E5=AD=A6=E5=BA=93=20?= =?UTF-8?q?=E4=BB=BF=E7=9C=9F=E5=BE=AA=E7=8E=AF=E4=B8=AD=E6=96=B0=E5=A2=9E?= =?UTF-8?q?=E5=85=B3=E8=8A=82=E6=8E=A7=E5=88=B6=E4=BB=A3=E7=A0=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../humanoid_simulation.py | 23 +++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/src/humanoid_3d_animation/humanoid_simulation.py b/src/humanoid_3d_animation/humanoid_simulation.py index 68cdeedf1b..be34f09ab4 100644 --- a/src/humanoid_3d_animation/humanoid_simulation.py +++ b/src/humanoid_3d_animation/humanoid_simulation.py @@ -2,6 +2,8 @@ import mujoco.viewer as viewer import os import time +# 新增：导入数学库，用于生成周期性的正弦/余弦运动信号 +import math def create_humanoid_xml(file_path): """自动创建humanoid.xml文件并写入模型代码""" @@ -129,6 +131,27 @@ def run_humanoid_simulation(): step_interval = 0.01 print("仿真开始，按窗口关闭按钮退出...") for _ in range(sim_steps): + # ========== 新增：关节主动运动控制核心代码 ========== + # 1. 手臂运动：左肩关节和右肩关节做相反的正弦运动（周期性摆动） + # data.time：仿真累计时间（秒），驱动周期性运动 + # math.sin(data.time * 2)：2Hz频率的正弦波，取值[-1,1] + # * 1.0：运动幅度（可调整，越大摆动越剧烈） + data.ctrl[0] = math.sin(data.time * 2) * 1.0 # 左肩关节（对应actuator索引0） + data.ctrl[1] = -math.sin(data.time * 2) * 1.0 # 右肩关节（对应actuator索引1，负号表示反向） + + # 2. 肘部运动：跟随肩部运动，幅度更小 + data.ctrl[2] = math.sin(data.time * 2) * 0.5 # 左肘部（索引2） + data.ctrl[3] = -math.sin(data.time * 2) * 0.5 # 右肘部（索引3） + + # 3. 腿部运动：左髋和右髋做余弦运动（与正弦波相位差90°，运动更协调） + data.ctrl[4] = math.cos(data.time * 1) * 0.8 # 左髋（索引4） + data.ctrl[5] = -math.cos(data.time * 1) * 0.8 # 右髋（索引5） + + # 4. 膝盖运动：跟随髋部运动，幅度稍小 + data.ctrl[6] = math.cos(data.time * 1) * 0.6 # 左膝盖（索引6） + data.ctrl[7] = -math.cos(data.time * 1) * 0.6 # 右膝盖（索引7） + # ================================================ + mujoco.mj_step(model, data) v.sync() time.sleep(step_interval) From 7cb0dae6d15589bcf88ad37de7ff7f8dfc79fc65 Mon Sep 17 00:00:00 2001 From: oyzw2302 Date: Sun, 21 Dec 2025 12:49:35 +0800 Subject: [PATCH 03/12] =?UTF-8?q?=E4=BC=98=E5=8C=96=E4=BB=BF=E7=9C=9F?= =?UTF-8?q?=E4=BA=BA?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/humanoid_3d_animation/smpl_humanoid.py | 666 +++++++++++++++++++++ 1 file changed, 666 insertions(+) create mode 100644 src/humanoid_3d_animation/smpl_humanoid.py diff --git a/src/humanoid_3d_animation/smpl_humanoid.py b/src/humanoid_3d_animation/smpl_humanoid.py new file mode 100644 index 0000000000..ad7c806907 --- /dev/null +++ b/src/humanoid_3d_animation/smpl_humanoid.py @@ -0,0 +1,666 @@ +import mujoco +import mujoco.viewer as viewer +import numpy as np +import time # 导入time库，用于实现等待功能 + +def main(): + # ========================================================================== + # 第一步：把你的XML内容粘贴到下面的这个字符串变量中（替换掉示例的简单XML） + # 注意：保持三引号的包裹格式，XML内容直接放在中间即可 + # ========================================================================== + xml_content = """ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + """ + # ========================================================================== + # XML内容粘贴结束位置 + # ========================================================================== + + # 从XML字符串加载模型 + try: + model = mujoco.MjModel.from_xml_string(xml_content) + data = mujoco.MjData(model) + print("✅ MuJoCo模型加载成功！") + except Exception as e: + print(f"❌ 模型加载失败：{e}") + return + + # 运行仿真可视化 + with viewer.launch_passive(model, data) as v: + # 设置相机视角（可根据需要调整） + v.cam.azimuth = 45 + v.cam.elevation = -20 + v.cam.distance = 3.0 + v.cam.lookat = np.array([0.0, 0.0, 1.0]) + + # 仿真循环 + while v.is_running(): + # 修复：正确判断关节是否存在并获取关节ID的方式 + t = data.time + # 方法1：使用try-except捕获关节不存在的异常（推荐） + try: + # 获取L_Hip_y关节的ID + l_hip_y_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, "L_Hip_y") + if l_hip_y_id >= 0: # 确认ID有效（>=0表示存在） + data.ctrl[l_hip_y_id] = 0.5 * np.sin(t) + except: + # 关节不存在时跳过，不报错 + pass + + try: + # 获取R_Hip_y关节的ID + r_hip_y_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, "R_Hip_y") + if r_hip_y_id >= 0: + data.ctrl[r_hip_y_id] = 0.5 * np.cos(t) + except: + pass + + # 执行一步仿真 + mujoco.mj_step(model, data) + # 更新可视化 + v.sync() + # 修复：用time.sleep替代mujoco.mj_wait，实现相同的等待功能 + time.sleep(model.opt.timestep) + +if __name__ == "__main__": + main() \ No newline at end of file From 7c4e76cd18ffd669cf69e50c940047f429aa5c58 Mon Sep 17 00:00:00 2001 From: oyzw2302 Date: Sun, 21 Dec 2025 14:33:04 +0800 Subject: [PATCH 04/12] =?UTF-8?q?=E4=B8=BA=E5=AF=BC=E5=85=A5=E7=9A=84?= =?UTF-8?q?=E5=BA=93=E6=B7=BB=E5=8A=A0=E6=B3=A8=E9=87=8A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/humanoid_3d_animation/smpl_humanoid.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/humanoid_3d_animation/smpl_humanoid.py b/src/humanoid_3d_animation/smpl_humanoid.py index ad7c806907..30348913dd 100644 --- a/src/humanoid_3d_animation/smpl_humanoid.py +++ b/src/humanoid_3d_animation/smpl_humanoid.py @@ -1,6 +1,6 @@ -import mujoco -import mujoco.viewer as viewer -import numpy as np +import mujoco # 导入MuJoCo核心库，用于加载模型和运行仿真 +import mujoco.viewer as viewer # 导入MuJoCo的可视化器 +import numpy as np # 导入numpy库，用于数值计算（如三角函数、数组操作） import time # 导入time库，用于实现等待功能 def main(): From 59c74288b560925ef4c99c536b09239ca94d78a4 Mon Sep 17 00:00:00 2001 From: oyzw2302 Date: Sun, 21 Dec 2025 21:26:03 +0800 Subject: [PATCH 05/12] =?UTF-8?q?=E4=BB=A3=E7=A0=81=E4=BC=98=E5=8C=96=20?= =?UTF-8?q?=E6=96=B0=E5=A2=9E=E5=8A=9F=E8=83=BD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../humanoid_simulation.py | 229 ++++++++++++++---- 1 file changed, 185 insertions(+), 44 deletions(-) diff --git a/src/humanoid_3d_animation/humanoid_simulation.py b/src/humanoid_3d_animation/humanoid_simulation.py index be34f09ab4..92f4605c1f 100644 --- a/src/humanoid_3d_animation/humanoid_simulation.py +++ b/src/humanoid_3d_animation/humanoid_simulation.py @@ -2,34 +2,46 @@ import mujoco.viewer as viewer import os import time -# 新增：导入数学库，用于生成周期性的正弦/余弦运动信号 import math +import threading # 新增：用于监听控制台输入（实现重置指令） +import numpy as np def create_humanoid_xml(file_path): - """自动创建humanoid.xml文件并写入模型代码""" + """ + 自动创建humanoid.xml文件并写入模型代码 + 优化点：XML内容格式化，增加注释，提升可读性 + """ xml_content = """ + + """ with open(file_path, "w", encoding="utf-8") as f: f.write(xml_content) - print(f"已自动在 {file_path} 创建humanoid.xml文件！") + print(f"✅ 已自动在 {file_path} 创建humanoid.xml文件！") + +def get_joint_ctrl_id(model, joint_name): + """ + 根据关节名称获取对应的控制索引（替代硬编码索引，提升鲁棒性） + 参数： + model: MuJoCo的MjModel对象 + joint_name: 关节名称（如"left_shoulder"） + 返回： + 控制索引（int），若不存在返回-1 + """ + # 先获取电机执行器的ID（对应actuator中的motor） + motor_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"{joint_name}_motor") + if motor_id == -1: + # 若没有电机，尝试获取阻尼执行器ID（兼容旧版） + motor_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, joint_name) + return motor_id + +def print_robot_state(data, joint_names, interval=1.0): + """ + 周期性打印机器人关节状态（位置、控制信号） + 参数： + data: MuJoCo的MjData对象 + joint_names: 需要打印的关节名称列表 + interval: 打印时间间隔（秒） + """ + current_time = data.time + if not hasattr(print_robot_state, "last_print_time"): + print_robot_state.last_print_time = 0.0 # 初始化上次打印时间 + + if current_time - print_robot_state.last_print_time >= interval: + print(f"\n===== 机器人状态（时间：{current_time:.2f}s）=====") + for name in joint_names: + # 获取关节ID和控制索引 + joint_id = mujoco.mj_name2id(data.model, mujoco.mjtObj.mjOBJ_JOINT, name) + ctrl_id = get_joint_ctrl_id(data.model, name) + if joint_id != -1 and ctrl_id != -1: + # 根关节是自由关节（7个自由度），普通关节的qpos索引偏移7位 + qpos_index = 7 + joint_id # 自由关节占前7个qpos + if qpos_index < len(data.qpos): + print(f"关节 {name}: 位置 = {data.qpos[qpos_index]:.2f} rad, 控制信号 = {data.ctrl[ctrl_id]:.2f}") + print_robot_state.last_print_time = current_time + +def reset_robot(model, data): + """ + 重置机器人到初始状态 + 参数： + model: MuJoCo的MjModel对象 + data: MuJoCo的MjData对象 + """ + mujoco.mj_resetData(model, data) # 重置动力学数据 + data.qpos[0:7] = [0, 0, 1.0, 1, 0, 0, 0] # 重置根关节位置（x,y,z,四元数） + print("\n🔄 机器人已重置到初始状态！") + +def input_listener(reset_flag): + """ + 后台线程：监听控制台输入，输入'r'则设置重置标记 + 参数： + reset_flag: 共享的布尔列表（用于跨线程传递标记，列表是可变对象） + """ + while True: + user_input = input().strip().lower() + if user_input == 'r': + reset_flag[0] = True + elif user_input == 'q': + print("📤 收到退出指令，仿真将结束...") + break def run_humanoid_simulation(): - # 直接写死桌面路径（替换成你的实际用户名，这里保留你的用户名） - # 注意：路径中的反斜杠用双反斜杠，或单反斜杠加r前缀 - model_path = r"C:\Users\欧阳志威1\Desktop\humanoid.xml" # r前缀表示原始字符串，避免转义 + """ + 优化后的仿真主函数：修复API兼容问题，用控制台输入实现重置 + """ + # 优化：使用用户目录拼接路径，避免硬编码用户名（更通用） + desktop_path = os.path.join(os.path.expanduser("~"), "Desktop") + model_path = os.path.join(desktop_path, "humanoid.xml") - # 打印路径 + # 打印路径信息 print(f"===== 模型文件路径 =====") print(f"模型文件完整路径：{model_path}") print(f"========================") @@ -98,64 +194,109 @@ def run_humanoid_simulation(): if not os.path.exists(model_path): create_humanoid_xml(model_path) else: - print("humanoid.xml文件已存在，无需重新创建！") + print("ℹ️ humanoid.xml文件已存在，无需重新创建！") - # 加载模型（新增：用Python内置的open函数测试是否能读取文件） + # 加载模型：直接读取内容，用字符串加载（彻底解决中文路径问题） try: - # 先测试Python是否能读取该文件（排除系统权限问题） with open(model_path, "r", encoding="utf-8") as f: - f.read() - print("Python内置函数已成功读取文件，权限正常！") + xml_content = f.read() + print("✅ Python内置函数已成功读取文件，权限正常！") except Exception as e: - print(f"Python读取文件失败，权限/路径问题：{e}") + print(f"❌ Python读取文件失败，权限/路径问题：{e}") return # 加载MuJoCo模型 try: - model = mujoco.MjModel.from_xml_path(model_path) + model = mujoco.MjModel.from_xml_string(xml_content) data = mujoco.MjData(model) - print("模型加载成功！开始启动仿真...") + print("✅ 从字符串加载模型成功！开始启动仿真...") except Exception as e: - print(f"MuJoCo加载模型失败：{e}") - # 备选方案：从字符串加载模型（绕过文件读取） - print("尝试从字符串直接加载模型...") - with open(model_path, "r", encoding="utf-8") as f: - xml_str = f.read() - model = mujoco.MjModel.from_xml_string(xml_str) - data = mujoco.MjData(model) - print("从字符串加载模型成功！开始启动仿真...") + print(f"❌ 模型加载失败：{e}") + return - # 运行仿真 + # 定义需要控制的关节名称 + joint_names = [ + "left_shoulder", "right_shoulder", + "left_elbow", "right_elbow", + "left_hip", "right_hip", + "left_knee", "right_knee" + ] + + # 新增：共享重置标记（用列表实现跨线程可变对象） + reset_flag = [False] + # 启动后台线程监听控制台输入 + input_thread = threading.Thread(target=input_listener, args=(reset_flag,), daemon=True) + input_thread.start() + + # 运行仿真可视化 with viewer.launch_passive(model, data) as v: - sim_steps = 10000 - step_interval = 0.01 - print("仿真开始，按窗口关闭按钮退出...") - for _ in range(sim_steps): - # ========== 新增：关节主动运动控制核心代码 ========== - # 1. 手臂运动：左肩关节和右肩关节做相反的正弦运动（周期性摆动） - # data.time：仿真累计时间（秒），驱动周期性运动 - # math.sin(data.time * 2)：2Hz频率的正弦波，取值[-1,1] - # * 1.0：运动幅度（可调整，越大摆动越剧烈） - data.ctrl[0] = math.sin(data.time * 2) * 1.0 # 左肩关节（对应actuator索引0） - data.ctrl[1] = -math.sin(data.time * 2) * 1.0 # 右肩关节（对应actuator索引1，负号表示反向） + # 相机跟随设置（跟随骨盆位置） + pelvis_body_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "pelvis") + if pelvis_body_id != -1: + v.cam.trackbodyid = pelvis_body_id # 跟踪骨盆体 + v.cam.distance = 2.0 # 相机距离跟随目标的距离 + v.cam.azimuth = 45 # 相机方位角 + v.cam.elevation = -20 # 相机仰角 + + print("\n📌 仿真操作提示：") + print(" - 在控制台输入 'r' 并回车，重置机器人到初始状态") + print(" - 在控制台输入 'q' 并回车，退出仿真") + print(" - 关闭可视化窗口也可退出仿真") + + print("🚀 仿真开始...") + + while v.is_running(): + # 检查重置标记：如果为True，执行重置并重置标记 + if reset_flag[0]: + reset_robot(model, data) + reset_flag[0] = False # 重置标记置为False + + # ========== 关节主动运动控制（用关节名称获取索引） ========== + t = data.time # 仿真累计时间 + # 1. 手臂运动：左肩关节和右肩关节做相反的正弦运动（2Hz频率） + left_shoulder_id = get_joint_ctrl_id(model, "left_shoulder") + right_shoulder_id = get_joint_ctrl_id(model, "right_shoulder") + if left_shoulder_id != -1: + data.ctrl[left_shoulder_id] = math.sin(t * 2) * 1.0 # 左肩关节 + if right_shoulder_id != -1: + data.ctrl[right_shoulder_id] = -math.sin(t * 2) * 1.0 # 右肩关节（反向） # 2. 肘部运动：跟随肩部运动，幅度更小 - data.ctrl[2] = math.sin(data.time * 2) * 0.5 # 左肘部（索引2） - data.ctrl[3] = -math.sin(data.time * 2) * 0.5 # 右肘部（索引3） + left_elbow_id = get_joint_ctrl_id(model, "left_elbow") + right_elbow_id = get_joint_ctrl_id(model, "right_elbow") + if left_elbow_id != -1: + data.ctrl[left_elbow_id] = math.sin(t * 2) * 0.5 # 左肘部 + if right_elbow_id != -1: + data.ctrl[right_elbow_id] = -math.sin(t * 2) * 0.5 # 右肘部（反向） - # 3. 腿部运动：左髋和右髋做余弦运动（与正弦波相位差90°，运动更协调） - data.ctrl[4] = math.cos(data.time * 1) * 0.8 # 左髋（索引4） - data.ctrl[5] = -math.cos(data.time * 1) * 0.8 # 右髋（索引5） + # 3. 腿部运动：左髋和右髋做余弦运动（2Hz频率，和手臂同步） + left_hip_id = get_joint_ctrl_id(model, "left_hip") + right_hip_id = get_joint_ctrl_id(model, "right_hip") + if left_hip_id != -1: + data.ctrl[left_hip_id] = math.cos(t * 2) * 0.8 # 左髋 + if right_hip_id != -1: + data.ctrl[right_hip_id] = -math.cos(t * 2) * 0.8 # 右髋（反向） # 4. 膝盖运动：跟随髋部运动，幅度稍小 - data.ctrl[6] = math.cos(data.time * 1) * 0.6 # 左膝盖（索引6） - data.ctrl[7] = -math.cos(data.time * 1) * 0.6 # 右膝盖（索引7） + left_knee_id = get_joint_ctrl_id(model, "left_knee") + right_knee_id = get_joint_ctrl_id(model, "right_knee") + if left_knee_id != -1: + data.ctrl[left_knee_id] = math.cos(t * 2) * 0.6 # 左膝盖 + if right_knee_id != -1: + data.ctrl[right_knee_id] = -math.cos(t * 2) * 0.6 # 右膝盖（反向） # ================================================ + # 执行仿真步 mujoco.mj_step(model, data) + # 更新可视化 v.sync() - time.sleep(step_interval) - print("仿真结束！") + # 控制仿真速度（使用模型时间步长，更匹配物理仿真） + time.sleep(model.opt.timestep) + + # 周期性打印机器人状态（每1秒打印一次） + print_robot_state(data, joint_names, interval=1.0) + + print("\n🏁 仿真结束！") if __name__ == "__main__": run_humanoid_simulation() \ No newline at end of file From 93595d30371f5500349f7a6584a6750dc55f81d3 Mon Sep 17 00:00:00 2001 From: oyzw2302 Date: Mon, 22 Dec 2025 10:58:32 +0800 Subject: [PATCH 06/12] =?UTF-8?q?=E9=85=8D=E7=BD=AE=E5=8C=96=E4=B8=8E?= =?UTF-8?q?=E6=A8=A1=E5=9D=97=E5=8C=96,=20=E6=80=A7=E8=83=BD=E4=BC=98?= =?UTF-8?q?=E5=8C=96=20,=E5=8A=9F=E8=83=BD=E6=89=A9=E5=B1=95=E4=B8=8E?= =?UTF-8?q?=E7=94=A8=E6=88=B7=E4=BD=93=E9=AA=8C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../humanoid_simulation.py | 491 ++++++++++-------- 1 file changed, 267 insertions(+), 224 deletions(-) diff --git a/src/humanoid_3d_animation/humanoid_simulation.py b/src/humanoid_3d_animation/humanoid_simulation.py index 92f4605c1f..c29ef757f3 100644 --- a/src/humanoid_3d_animation/humanoid_simulation.py +++ b/src/humanoid_3d_animation/humanoid_simulation.py @@ -3,45 +3,82 @@ import os import time import math -import threading # 新增：用于监听控制台输入（实现重置指令） -import numpy as np - -def create_humanoid_xml(file_path): - """ - 自动创建humanoid.xml文件并写入模型代码 - 优化点：XML内容格式化，增加注释，提升可读性 - """ - xml_content = """ - +import threading +import signal +import sys +from dataclasses import dataclass # 用于配置类 + +# ====================== 配置抽离（核心优化点）====================== +@dataclass +class SimConfig: + """仿真配置类：集中管理所有可配置参数""" + # 文件路径配置 + xml_filename: str = "humanoid.xml" + # 仿真参数 + timestep: float = 0.005 # 与XML中的timestep保持一致 + sim_frequency: float = 2.0 # 关节运动频率（Hz） + state_print_interval: float = 1.0 # 状态打印间隔（秒） + # 相机参数 + cam_distance: float = 2.0 + cam_azimuth: float = 45.0 + cam_elevation: float = -20.0 + # 关节运动幅度配置 + joint_amplitudes = { + "left_shoulder": 1.0, "right_shoulder": 1.0, + "left_elbow": 0.5, "right_elbow": 0.5, + "left_hip": 0.8, "right_hip": 0.8, + "left_knee": 0.6, "right_knee": 0.6 + } + # 控制模式：sin（正弦运动）、random（随机运动）、stop（静止） + default_mode: str = "sin" + +# 全局变量：用于优雅退出 +sim_running = True + +def signal_handler(sig, frame): + """处理Ctrl+C中断信号，实现优雅退出""" + global sim_running + sim_running = False + print("\n⚠️ 收到中断信号，正在退出仿真...") + sys.exit(0) + +# 注册信号处理 +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()) + # 预存关节ID和控制ID（避免每次循环重复计算，性能优化） + self.joint_ctrl_ids = {} + self.joint_qpos_indices = {} + # 运动模式和控制信号缓存（用于平滑控制） + self.current_mode = config.default_mode + self.last_ctrl_signals = {} # 存储上一帧的控制信号 + + def create_xml_file(self, file_path): + """创建人形机器人XML文件""" + xml_content = f""" - - """ - with open(file_path, "w", encoding="utf-8") as f: - f.write(xml_content) - print(f"✅ 已自动在 {file_path} 创建humanoid.xml文件！") - -def get_joint_ctrl_id(model, joint_name): - """ - 根据关节名称获取对应的控制索引（替代硬编码索引，提升鲁棒性） - 参数： - model: MuJoCo的MjModel对象 - joint_name: 关节名称（如"left_shoulder"） - 返回： - 控制索引（int），若不存在返回-1 - """ - # 先获取电机执行器的ID（对应actuator中的motor） - motor_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, f"{joint_name}_motor") - if motor_id == -1: - # 若没有电机，尝试获取阻尼执行器ID（兼容旧版） - motor_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, joint_name) - return motor_id - -def print_robot_state(data, joint_names, interval=1.0): - """ - 周期性打印机器人关节状态（位置、控制信号） - 参数： - data: MuJoCo的MjData对象 - joint_names: 需要打印的关节名称列表 - interval: 打印时间间隔（秒） - """ - current_time = data.time - if not hasattr(print_robot_state, "last_print_time"): - print_robot_state.last_print_time = 0.0 # 初始化上次打印时间 - - if current_time - print_robot_state.last_print_time >= interval: - print(f"\n===== 机器人状态（时间：{current_time:.2f}s）=====") - for name in joint_names: - # 获取关节ID和控制索引 - joint_id = mujoco.mj_name2id(data.model, mujoco.mjtObj.mjOBJ_JOINT, name) - ctrl_id = get_joint_ctrl_id(data.model, name) - if joint_id != -1 and ctrl_id != -1: - # 根关节是自由关节（7个自由度），普通关节的qpos索引偏移7位 - qpos_index = 7 + joint_id # 自由关节占前7个qpos - if qpos_index < len(data.qpos): - print(f"关节 {name}: 位置 = {data.qpos[qpos_index]:.2f} rad, 控制信号 = {data.ctrl[ctrl_id]:.2f}") - print_robot_state.last_print_time = current_time - -def reset_robot(model, data): - """ - 重置机器人到初始状态 - 参数： - model: MuJoCo的MjModel对象 - data: MuJoCo的MjData对象 - """ - mujoco.mj_resetData(model, data) # 重置动力学数据 - data.qpos[0:7] = [0, 0, 1.0, 1, 0, 0, 0] # 重置根关节位置（x,y,z,四元数） - print("\n🔄 机器人已重置到初始状态！") - -def input_listener(reset_flag): - """ - 后台线程：监听控制台输入，输入'r'则设置重置标记 - 参数： - reset_flag: 共享的布尔列表（用于跨线程传递标记，列表是可变对象） - """ - while True: - user_input = input().strip().lower() - if user_input == 'r': - reset_flag[0] = True - elif user_input == 'q': - print("📤 收到退出指令，仿真将结束...") - break - -def run_humanoid_simulation(): - """ - 优化后的仿真主函数：修复API兼容问题，用控制台输入实现重置 - """ - # 优化：使用用户目录拼接路径，避免硬编码用户名（更通用） - desktop_path = os.path.join(os.path.expanduser("~"), "Desktop") - model_path = os.path.join(desktop_path, "humanoid.xml") - - # 打印路径信息 - print(f"===== 模型文件路径 =====") - print(f"模型文件完整路径：{model_path}") - print(f"========================") - - # 检查并创建文件 - if not os.path.exists(model_path): - create_humanoid_xml(model_path) - else: - print("ℹ️ humanoid.xml文件已存在，无需重新创建！") - - # 加载模型：直接读取内容，用字符串加载（彻底解决中文路径问题） - try: - with open(model_path, "r", encoding="utf-8") as f: - xml_content = f.read() - print("✅ Python内置函数已成功读取文件，权限正常！") - except Exception as e: - print(f"❌ Python读取文件失败，权限/路径问题：{e}") - return - - # 加载MuJoCo模型 - try: - model = mujoco.MjModel.from_xml_string(xml_content) - data = mujoco.MjData(model) - print("✅ 从字符串加载模型成功！开始启动仿真...") - except Exception as e: - print(f"❌ 模型加载失败：{e}") - return - - # 定义需要控制的关节名称 - joint_names = [ - "left_shoulder", "right_shoulder", - "left_elbow", "right_elbow", - "left_hip", "right_hip", - "left_knee", "right_knee" - ] - - # 新增：共享重置标记（用列表实现跨线程可变对象） - reset_flag = [False] - # 启动后台线程监听控制台输入 - input_thread = threading.Thread(target=input_listener, args=(reset_flag,), daemon=True) - input_thread.start() - - # 运行仿真可视化 - with viewer.launch_passive(model, data) as v: - # 相机跟随设置（跟随骨盆位置） - pelvis_body_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, "pelvis") - if pelvis_body_id != -1: - v.cam.trackbodyid = pelvis_body_id # 跟踪骨盆体 - v.cam.distance = 2.0 # 相机距离跟随目标的距离 - v.cam.azimuth = 45 # 相机方位角 - v.cam.elevation = -20 # 相机仰角 - - print("\n📌 仿真操作提示：") - print(" - 在控制台输入 'r' 并回车，重置机器人到初始状态") - print(" - 在控制台输入 'q' 并回车，退出仿真") - print(" - 关闭可视化窗口也可退出仿真") - - print("🚀 仿真开始...") - - while v.is_running(): - # 检查重置标记：如果为True，执行重置并重置标记 - if reset_flag[0]: - reset_robot(model, data) - reset_flag[0] = False # 重置标记置为False - - # ========== 关节主动运动控制（用关节名称获取索引） ========== - t = data.time # 仿真累计时间 - # 1. 手臂运动：左肩关节和右肩关节做相反的正弦运动（2Hz频率） - left_shoulder_id = get_joint_ctrl_id(model, "left_shoulder") - right_shoulder_id = get_joint_ctrl_id(model, "right_shoulder") - if left_shoulder_id != -1: - data.ctrl[left_shoulder_id] = math.sin(t * 2) * 1.0 # 左肩关节 - if right_shoulder_id != -1: - data.ctrl[right_shoulder_id] = -math.sin(t * 2) * 1.0 # 右肩关节（反向） - - # 2. 肘部运动：跟随肩部运动，幅度更小 - left_elbow_id = get_joint_ctrl_id(model, "left_elbow") - right_elbow_id = get_joint_ctrl_id(model, "right_elbow") - if left_elbow_id != -1: - data.ctrl[left_elbow_id] = math.sin(t * 2) * 0.5 # 左肘部 - if right_elbow_id != -1: - data.ctrl[right_elbow_id] = -math.sin(t * 2) * 0.5 # 右肘部（反向） - - # 3. 腿部运动：左髋和右髋做余弦运动（2Hz频率，和手臂同步） - left_hip_id = get_joint_ctrl_id(model, "left_hip") - right_hip_id = get_joint_ctrl_id(model, "right_hip") - if left_hip_id != -1: - data.ctrl[left_hip_id] = math.cos(t * 2) * 0.8 # 左髋 - if right_hip_id != -1: - data.ctrl[right_hip_id] = -math.cos(t * 2) * 0.8 # 右髋（反向） - - # 4. 膝盖运动：跟随髋部运动，幅度稍小 - left_knee_id = get_joint_ctrl_id(model, "left_knee") - right_knee_id = get_joint_ctrl_id(model, "right_knee") - if left_knee_id != -1: - data.ctrl[left_knee_id] = math.cos(t * 2) * 0.6 # 左膝盖 - if right_knee_id != -1: - data.ctrl[right_knee_id] = -math.cos(t * 2) * 0.6 # 右膝盖（反向） - # ================================================ - - # 执行仿真步 - mujoco.mj_step(model, data) - # 更新可视化 - v.sync() - # 控制仿真速度（使用模型时间步长，更匹配物理仿真） - time.sleep(model.opt.timestep) - - # 周期性打印机器人状态（每1秒打印一次） - print_robot_state(data, joint_names, interval=1.0) + with open(file_path, "w", encoding="utf-8") as f: + f.write(xml_content) + print(f"✅ 已在 {file_path} 创建XML文件！") + + def load_model(self): + """加载MuJoCo模型，预存关节ID和控制ID（性能优化）""" + # 获取文件路径 + desktop_path = os.path.join(os.path.expanduser("~"), "Desktop") + self.model_path = os.path.join(desktop_path, self.config.xml_filename) + + # 检查并创建文件 + if not os.path.exists(self.model_path): + self.create_xml_file(self.model_path) + else: + print("ℹ️ XML文件已存在，无需重新创建！") + + # 读取XML内容并加载模型（解决中文路径问题） + try: + with open(self.model_path, "r", encoding="utf-8") as f: + xml_content = f.read() + self.model = mujoco.MjModel.from_xml_string(xml_content) + self.data = mujoco.MjData(self.model) + print("✅ 模型加载成功！") + except Exception as e: + print(f"❌ 模型加载失败：{e}") + sys.exit(1) + + # 预存关节控制ID和qpos索引（只计算一次，性能优化） + 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 + else: + signal = math.cos(t * freq) * amplitude + else: + if "shoulder" in name or "elbow" in name: + signal = -math.sin(t * freq) * amplitude + 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 # 平滑系数，越小越平滑 + 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 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}") + 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 + print("\n🔄 机器人已重置到初始状态！") + + def input_listener(self): + """后台线程：监听控制台输入，支持多指令（功能扩展）""" + global sim_running + while sim_running: + try: + user_input = input().strip().lower() + if user_input == 'r': + self.reset_robot() + elif user_input in ["sin", "random", "stop"]: + self.current_mode = user_input + print(f"\n🔄 运动模式已切换为：{user_input}") + elif user_input == 'q': + sim_running = False + print("\n📤 收到退出指令，仿真将结束...") + else: + print(f"\n❓ 未知指令：{user_input}，支持的指令：r（重置）、sin/random/stop（模式）、q（退出）") + except EOFError: + continue + except Exception as e: + print(f"\n⚠️ 输入处理失败：{e}") + + def run_simulation(self): + """运行仿真主循环""" + # 加载模型 + self.load_model() + + # 启动输入监听线程 + input_thread = threading.Thread(target=self.input_listener, daemon=True) + input_thread.start() + + # 启动可视化 + 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 + v.cam.distance = self.config.cam_distance + v.cam.azimuth = self.config.cam_azimuth + v.cam.elevation = self.config.cam_elevation + + # 打印操作提示（用户体验优化） + print("\n📌 仿真操作提示：") + print(" - 输入 'r' 回车：重置机器人") + print(" - 输入 'sin'/'random'/'stop' 回车：切换运动模式") + 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: + print(f"\n⚠️ 仿真步执行失败：{e}") + self.reset_robot() + + # 更新可视化 + v.sync() + + # 打印状态 + self.print_robot_state() + + last_step_time = current_time print("\n🏁 仿真结束！") +# ====================== 程序入口 ====================== if __name__ == "__main__": - run_humanoid_simulation() \ No newline at end of file + # 初始化配置 + config = SimConfig() + # 创建仿真器并运行 + simulator = HumanoidSimulator(config) + simulator.run_simulation() \ No newline at end of file From e52c5884598cb35efaace523e314c2c724405167 Mon Sep 17 00:00:00 2001 From: oyzw2302 Date: Tue, 23 Dec 2025 15:23:23 +0800 Subject: [PATCH 07/12] =?UTF-8?q?=E6=96=B0=E5=A2=9Ehumanoid.xml=E6=96=87?= =?UTF-8?q?=E4=BB=B6=EF=BC=8C=E4=BF=AE=E6=94=B9=E4=BA=86humanoid=5Fsimulat?= =?UTF-8?q?ion.py=E8=AF=BB=E5=8F=96humanoid.xml=E7=9A=84=E8=B7=AF=E5=BE=84?= =?UTF-8?q?=EF=BC=8C=E4=BD=BF=E4=BB=A3=E7=A0=81=E8=83=BD=E7=9B=B4=E6=8E=A5?= =?UTF-8?q?=E8=BF=90=E8=A1=8C=EF=BC=8C=E8=80=8C=E6=97=A0=E9=9C=80=E5=9C=A8?= =?UTF-8?q?Desktop=E5=88=9B=E5=BB=BA?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/humanoid_3d_animation/humanoid.xml | 121 ++++++++++++++++++ .../humanoid_simulation.py | 28 ++-- 2 files changed, 135 insertions(+), 14 deletions(-) create mode 100644 src/humanoid_3d_animation/humanoid.xml diff --git a/src/humanoid_3d_animation/humanoid.xml b/src/humanoid_3d_animation/humanoid.xml new file mode 100644 index 0000000000..a263bdbcf5 --- /dev/null +++ b/src/humanoid_3d_animation/humanoid.xml @@ -0,0 +1,121 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/humanoid_3d_animation/humanoid_simulation.py b/src/humanoid_3d_animation/humanoid_simulation.py index c29ef757f3..7d50c852cb 100644 --- a/src/humanoid_3d_animation/humanoid_simulation.py +++ b/src/humanoid_3d_animation/humanoid_simulation.py @@ -12,7 +12,7 @@ @dataclass class SimConfig: """仿真配置类：集中管理所有可配置参数""" - # 文件路径配置 + # 文件路径配置（修改：XML文件在当前项目目录下） xml_filename: str = "humanoid.xml" # 仿真参数 timestep: float = 0.005 # 与XML中的timestep保持一致 @@ -120,22 +120,22 @@ def create_xml_file(self, file_path): - + < - + < - + < - + < - + < - + < - + < - + < """ with open(file_path, "w", encoding="utf-8") as f: @@ -144,17 +144,17 @@ def create_xml_file(self, file_path): def load_model(self): """加载MuJoCo模型，预存关节ID和控制ID（性能优化）""" - # 获取文件路径 - desktop_path = os.path.join(os.path.expanduser("~"), "Desktop") - self.model_path = os.path.join(desktop_path, self.config.xml_filename) + # 核心修改：获取当前项目目录（即脚本所在的文件夹路径） + 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("ℹ️ XML文件已存在，无需重新创建！") + print(f"ℹ️ XML文件已存在（路径：{self.model_path}），无需重新创建！") - # 读取XML内容并加载模型（解决中文路径问题） + # 读取XML内容并加载模型 try: with open(self.model_path, "r", encoding="utf-8") as f: xml_content = f.read() From f561f3fbb37de9cb5bf61f71607ec3e183d73f90 Mon Sep 17 00:00:00 2001 From: oyzw2302 Date: Tue, 23 Dec 2025 17:36:16 +0800 Subject: [PATCH 08/12] =?UTF-8?q?=E4=BF=AE=E5=A4=8D=E7=A8=8B=E5=BA=8F?= =?UTF-8?q?=E9=80=80=E5=87=BA=E6=97=B6=E5=87=BA=E7=8E=B0=E7=9A=84=E9=97=AE?= =?UTF-8?q?=E9=A2=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../humanoid_simulation.py | 80 ++++++++++++------- 1 file changed, 51 insertions(+), 29 deletions(-) diff --git a/src/humanoid_3d_animation/humanoid_simulation.py b/src/humanoid_3d_animation/humanoid_simulation.py index 7d50c852cb..d1e74460a1 100644 --- a/src/humanoid_3d_animation/humanoid_simulation.py +++ b/src/humanoid_3d_animation/humanoid_simulation.py @@ -6,13 +6,14 @@ import threading import signal import sys +import select # 新增：用于非阻塞输入监听 from dataclasses import dataclass # 用于配置类 # ====================== 配置抽离（核心优化点）====================== @dataclass class SimConfig: """仿真配置类：集中管理所有可配置参数""" - # 文件路径配置（修改：XML文件在当前项目目录下） + # 文件路径配置（XML文件在当前脚本所在目录） xml_filename: str = "humanoid.xml" # 仿真参数 timestep: float = 0.005 # 与XML中的timestep保持一致 @@ -40,7 +41,7 @@ def signal_handler(sig, frame): global sim_running sim_running = False print("\n⚠️ 收到中断信号，正在退出仿真...") - sys.exit(0) + # 这里不再直接sys.exit，让主循环自然退出 # 注册信号处理 signal.signal(signal.SIGINT, signal_handler) @@ -58,6 +59,8 @@ def __init__(self, config: SimConfig): # 运动模式和控制信号缓存（用于平滑控制） self.current_mode = config.default_mode self.last_ctrl_signals = {} # 存储上一帧的控制信号 + # 新增：线程停止标记（用于优雅停止输入监听线程） + self.input_thread_running = False def create_xml_file(self, file_path): """创建人形机器人XML文件""" @@ -120,22 +123,22 @@ def create_xml_file(self, file_path): - < + - < + - < + - < + - < + - < + - < + - < + """ with open(file_path, "w", encoding="utf-8") as f: @@ -144,7 +147,7 @@ def create_xml_file(self, file_path): def load_model(self): """加载MuJoCo模型，预存关节ID和控制ID（性能优化）""" - # 核心修改：获取当前项目目录（即脚本所在的文件夹路径） + # 核心修改：获取当前脚本所在的文件夹路径 current_dir = os.path.dirname(os.path.abspath(__file__)) self.model_path = os.path.join(current_dir, self.config.xml_filename) @@ -262,33 +265,45 @@ def reset_robot(self): print("\n🔄 机器人已重置到初始状态！") def input_listener(self): - """后台线程：监听控制台输入，支持多指令（功能扩展）""" + """后台线程：非阻塞监听控制台输入（修复锁冲突问题）""" global sim_running - while sim_running: + # 设置线程运行标记 + self.input_thread_running = True + # 非阻塞读取的超时时间（避免线程空转） + timeout = 0.1 + + while self.input_thread_running and sim_running: try: - user_input = input().strip().lower() - if user_input == 'r': - self.reset_robot() - elif user_input in ["sin", "random", "stop"]: - self.current_mode = user_input - print(f"\n🔄 运动模式已切换为：{user_input}") - elif user_input == 'q': - sim_running = False - print("\n📤 收到退出指令，仿真将结束...") - else: - print(f"\n❓ 未知指令：{user_input}，支持的指令：r（重置）、sin/random/stop（模式）、q（退出）") - except EOFError: - continue + # 使用select检查是否有输入（非阻塞） + ready, _, _ = select.select([sys.stdin], [], [], timeout) + if ready: + # 读取输入（此时有数据，不会阻塞） + user_input = sys.stdin.readline().strip().lower() + if user_input == 'r': + self.reset_robot() + elif user_input in ["sin", "random", "stop"]: + self.current_mode = user_input + print(f"\n🔄 运动模式已切换为：{user_input}") + elif user_input == 'q': + sim_running = False + print("\n📤 收到退出指令，仿真将结束...") + elif user_input: # 非空的未知指令 + print(f"\n❓ 未知指令：{user_input}，支持的指令：r（重置）、sin/random/stop（模式）、q（退出）") except Exception as e: + # 捕获异常，避免线程崩溃 print(f"\n⚠️ 输入处理失败：{e}") + break + + # 线程退出提示（可选） + print("\n🔌 输入监听线程已优雅退出") def run_simulation(self): """运行仿真主循环""" # 加载模型 self.load_model() - # 启动输入监听线程 - input_thread = threading.Thread(target=self.input_listener, daemon=True) + # 启动输入监听线程（不再设置daemon=True，靠标记控制退出） + input_thread = threading.Thread(target=self.input_listener) input_thread.start() # 启动可视化 @@ -334,6 +349,11 @@ def run_simulation(self): last_step_time = current_time + # 优雅停止输入监听线程 + self.input_thread_running = False + # 等待线程退出（最多等待1秒，避免卡死） + input_thread.join(timeout=1.0) + print("\n🏁 仿真结束！") # ====================== 程序入口 ====================== @@ -342,4 +362,6 @@ def run_simulation(self): config = SimConfig() # 创建仿真器并运行 simulator = HumanoidSimulator(config) - simulator.run_simulation() \ No newline at end of file + simulator.run_simulation() + # 程序退出前清理（可选） + sys.exit(0) \ No newline at end of file From 7ce1f4ee6c5ef4528b931aa0ca2e156d49e55d3c Mon Sep 17 00:00:00 2001 From: oyzw2302 Date: Wed, 24 Dec 2025 16:21:47 +0800 Subject: [PATCH 09/12] =?UTF-8?q?=E6=96=B0=E5=A2=9E=E6=A0=B8=E5=BF=83?= =?UTF-8?q?=E5=8A=9F=E8=83=BD=EF=BC=9A=E5=85=B3=E8=8A=82=E8=A7=92=E5=BA=A6?= =?UTF-8?q?=E5=AE=9E=E6=97=B6=E5=8F=AF=E8=A7=86=E5=8C=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../humanoid_simulation.py | 227 +++++++++++++----- 1 file changed, 166 insertions(+), 61 deletions(-) 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 From 092f10dab359afed87ee50984a78ffa482808a62 Mon Sep 17 00:00:00 2001 From: oyzw2302 Date: Thu, 25 Dec 2025 01:01:12 +0800 Subject: [PATCH 10/12] =?UTF-8?q?=E6=96=B0=E5=A2=9E=E8=A1=8C=E8=B5=B0=20/?= =?UTF-8?q?=20=E6=8C=A5=E6=89=8B=E8=BF=90=E5=8A=A8=E6=A8=A1=E5=BC=8F,?= =?UTF-8?q?=E4=BF=AE=E5=A4=8D=20XML=20=E8=A7=A3=E6=9E=90=E9=94=99=E8=AF=AF?= =?UTF-8?q?=EF=BC=88perspective/damping/floor=20=E7=BA=A6=E6=9D=9F?= =?UTF-8?q?=EF=BC=89=EF=BC=8C=E5=85=BC=E5=AE=B9=E6=89=80=E6=9C=89=20MuJoCo?= =?UTF-8?q?=20=E7=89=88=E6=9C=AC?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/humanoid_3d_animation/humanoid.xml | 216 ++++----- .../humanoid_simulation.py | 446 ++++++++++++------ 2 files changed, 391 insertions(+), 271 deletions(-) diff --git a/src/humanoid_3d_animation/humanoid.xml b/src/humanoid_3d_animation/humanoid.xml index a263bdbcf5..881178f198 100644 --- a/src/humanoid_3d_animation/humanoid.xml +++ b/src/humanoid_3d_animation/humanoid.xml @@ -1,121 +1,99 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/humanoid_3d_animation/humanoid_simulation.py b/src/humanoid_3d_animation/humanoid_simulation.py index 4eb868e559..a8a5e77e61 100644 --- a/src/humanoid_3d_animation/humanoid_simulation.py +++ b/src/humanoid_3d_animation/humanoid_simulation.py @@ -6,7 +6,7 @@ import threading import signal import sys -import select +import random from dataclasses import dataclass import numpy as np import matplotlib.pyplot as plt @@ -17,8 +17,6 @@ @dataclass class SimConfig: """仿真配置类：集中管理所有可配置参数""" - # 文件路径配置 - xml_filename: str = "humanoid.xml" # 仿真参数 timestep: float = 0.005 sim_frequency: float = 2.0 @@ -27,18 +25,21 @@ class SimConfig: cam_distance: float = 2.0 cam_azimuth: float = 45.0 cam_elevation: float = -20.0 - # 关节运动幅度配置 + # 关节运动幅度配置（针对不同动作优化） joint_amplitudes = { - "left_shoulder": 1.0, "right_shoulder": 1.0, - "left_elbow": 0.5, "right_elbow": 0.5, - "left_hip": 0.8, "right_hip": 0.8, - "left_knee": 0.6, "right_knee": 0.6 + "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 = "sin" + # 控制模式（新增行走和挥手动作） + default_mode: str = "walk" # 可视化配置 plot_update_interval: int = 50 # 绘图更新间隔（帧数） max_plot_points: int = 200 # 图表最大显示数据点 + # 动作参数 + walk_stride: float = 0.8 # 行走步幅 + wave_frequency: float = 1.5 # 挥手频率 # 全局变量 @@ -68,9 +69,12 @@ def __init__(self, config: SimConfig): self.joint_qpos_indices = {} self.current_mode = config.default_mode self.last_ctrl_signals = {} - self.input_thread_running = False - # 新增：可视化相关变量 + # 新增：动作状态变量 + 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 @@ -80,150 +84,234 @@ def __init__(self, config: SimConfig): self.lines = {} self.ani = None - def create_xml_file(self, file_path): - """创建人形机器人XML文件""" - xml_content = f""" + def load_model(self): + """加载MuJoCo模型（完全修复XML格式）""" + # 完全兼容所有MuJoCo版本的XML + xml_content = """ - """ - with open(file_path, "w", encoding="utf-8") as f: - f.write(xml_content) - print(f"✅ 已在 {file_path} 创建XML文件！") - - def load_model(self): - """加载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}），无需重新创建！") + + +""" try: - with open(self.model_path, "r", encoding="utf-8") as f: - xml_content = f.read() + # 直接从XML字符串加载模型 self.model = mujoco.MjModel.from_xml_string(xml_content) self.data = mujoco.MjData(self.model) print("✅ 模型加载成功！") except Exception as e: print(f"❌ 模型加载失败：{e}") + import traceback + traceback.print_exc() sys.exit(1) - # 预存关节ID + # 映射关节ID + print("\n🔍 关节ID映射结果：") for name in self.joint_names: - 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) + # 直接使用关节名作为电机名 + ctrl_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, name) self.joint_ctrl_ids[name] = ctrl_id + # 获取关节ID和位置索引 joint_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, name) if joint_id != -1: - self.joint_qpos_indices[name] = 7 + joint_id + self.joint_qpos_indices[name] = self.model.jnt_qposadr[joint_id] else: self.joint_qpos_indices[name] = -1 self.last_ctrl_signals[name] = 0.0 - def get_joint_ctrl_signal(self, name, t): - """生成关节控制信号""" + # 打印详细信息 + print(f" {name}: ctrl_id={ctrl_id}, qpos_idx={self.joint_qpos_indices[name]}") + + # 验证控制信号数组 + print(f"\n📊 控制信号数组长度：{len(self.data.ctrl)}") + print(f"📊 关节位置数组长度：{len(self.data.qpos)}") + + def get_walk_action(self, name, t): + """生成行走动作控制信号""" amplitude = self.config.joint_amplitudes[name] - freq = self.config.sim_frequency + stride = self.config.walk_stride - 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 - else: - signal = math.cos(t * freq) * amplitude + # 更新行走相位 + self.walk_phase = (self.walk_phase + 0.01) % (2 * math.pi) + + if "hip" in name: + # 髋关节交替摆动 + if "left" in name: + signal = math.sin(self.walk_phase) * amplitude * stride else: - if "shoulder" in name or "elbow" in name: - signal = -math.sin(t * freq) * amplitude - else: - signal = -math.cos(t * freq) * amplitude + signal = math.sin(self.walk_phase + math.pi) * amplitude * stride + elif "knee" in name: + # 膝关节配合髋关节运动 + if "left" in name: + signal = math.cos(self.walk_phase) * amplitude * stride * 1.2 + else: + signal = math.cos(self.walk_phase + math.pi) * amplitude * stride * 1.2 + elif "shoulder" in name: + # 手臂自然摆动（与对侧腿相反） + if "left" in name: + signal = math.sin(self.walk_phase + math.pi) * amplitude * 0.5 + else: + signal = math.sin(self.walk_phase) * amplitude * 0.5 + elif "elbow" in name: + # 肘部轻微弯曲 + if "left" in name: + signal = -math.fabs(math.sin(self.walk_phase + math.pi)) * amplitude * 0.6 + else: + signal = -math.fabs(math.sin(self.walk_phase)) * amplitude * 0.6 + else: + signal = 0.0 + + return signal + + def get_wave_action(self, name, t): + """生成挥手动作控制信号""" + amplitude = self.config.joint_amplitudes[name] + freq = self.config.wave_frequency + + # 每2秒切换一次挥动手臂 + if int(t) % 2 == 0: + self.wave_arm = "right" + else: + self.wave_arm = "left" + + signal = 0.0 + + # 挥动手臂的肩部和肘部运动 + if f"{self.wave_arm}_shoulder" == name: + # 肩部上下摆动 + signal = math.sin(t * freq) * amplitude * 1.2 + elif f"{self.wave_arm}_elbow" == name: + # 肘部配合弯曲 + signal = -math.fabs(math.sin(t * freq)) * amplitude * 1.0 + # 另一只手臂保持自然下垂 + elif ("shoulder" in name and self.wave_arm not in name): + signal = -0.2 + elif ("elbow" in name and self.wave_arm not in name): + signal = -0.8 + # 腿部保持稳定 + elif "hip" in name or "knee" in name: + signal = 0.0 + + return signal + + def get_joint_ctrl_signal(self, name, t): + """生成关节控制信号（支持多种动作模式）""" + # 根据当前模式选择动作 + if self.current_mode == "walk": + signal = self.get_walk_action(name, t) + elif self.current_mode == "wave": + signal = self.get_wave_action(name, t) + elif self.current_mode == "sin": + # 原有正弦运动模式 + if "left" in name: + signal = math.sin(t * self.config.sim_frequency) * self.config.joint_amplitudes[name] + else: + signal = -math.sin(t * self.config.sim_frequency) * self.config.joint_amplitudes[name] elif self.current_mode == "random": - signal = (math.sin(t * freq * 0.5) * 0.5 + 0.5) * amplitude * 2 - amplitude + # 随机运动模式 + signal = (random.random() * 2 - 1) * self.config.joint_amplitudes[name] elif self.current_mode == "stop": + # 停止模式 signal = 0.0 else: signal = 0.0 # 平滑过渡 - smooth_factor = 0.1 + smooth_factor = 0.05 self.last_ctrl_signals[name] = (1 - smooth_factor) * self.last_ctrl_signals[name] + smooth_factor * signal + + # 限制信号范围在关节限位内 + joint_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, name) + if joint_id != -1: + jnt_range = self.model.jnt_range[joint_id] + self.last_ctrl_signals[name] = np.clip(self.last_ctrl_signals[name], jnt_range[0], jnt_range[1]) + return self.last_ctrl_signals[name] def update_joint_controls(self): @@ -233,9 +321,13 @@ def update_joint_controls(self): 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 + ctrl_signal = self.get_joint_ctrl_signal(name, t) + if 0 <= ctrl_id < len(self.data.ctrl): + self.data.ctrl[ctrl_id] = ctrl_signal + else: + print(f"⚠️ 关节 {name} 控制ID {ctrl_id} 超出范围（最大：{len(self.data.ctrl) - 1}）") except Exception as e: print(f"⚠️ 关节 {name} 控制失败：{e}") @@ -253,11 +345,13 @@ def collect_plot_data(self): # 添加各关节角度数据 for name in self.joint_names: qpos_idx = self.joint_qpos_indices[name] - if qpos_idx != -1 and qpos_idx < len(self.data.qpos): + if qpos_idx != -1 and 0 <= qpos_idx < len(self.data.qpos): angle = self.data.qpos[qpos_idx] self.plot_data[name].append(angle) + else: + self.plot_data[name].append(0.0) - # 限制数据点数量，避免内存占用过大 + # 限制数据点数量 if len(self.time_data) > self.config.max_plot_points: self.time_data.pop(0) for name in self.joint_names: @@ -284,8 +378,6 @@ def init_plot(self): 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() @@ -294,12 +386,10 @@ def init_plot(self): 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) @@ -319,13 +409,23 @@ def print_robot_state(self): 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）=====") + print( + f"\n===== 机器人状态（时间：{current_time:.2f}s | 帧率：{self.fps:.1f} FPS | 模式：{self.current_mode}）=====") 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): + + if ctrl_id != -1 and qpos_idx != -1 and qpos_idx < len(self.data.qpos) and ctrl_id < len( + self.data.ctrl): print( f"关节 {name}: 位置 = {self.data.qpos[qpos_idx]:.2f} rad, 控制信号 = {self.data.ctrl[ctrl_id]:.2f}") + elif ctrl_id == -1: + print(f"关节 {name}: 无控制ID") + elif qpos_idx == -1: + print(f"关节 {name}: 无位置索引") + else: + print(f"关节 {name}: 索引超出范围") + self.last_print_time = current_time def reset_robot(self): @@ -333,66 +433,82 @@ def reset_robot(self): 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 + ctrl_id = self.joint_ctrl_ids[name] + if ctrl_id != -1 and ctrl_id < len(self.data.ctrl): + self.data.ctrl[ctrl_id] = 0.0 + + 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 - print("\n🔄 机器人已重置到初始状态！") - def input_listener(self): - """后台线程：监听控制台输入""" - global sim_running - self.input_thread_running = True - timeout = 0.1 + print("\n🔄 机器人已重置到初始状态！") - while self.input_thread_running and sim_running: + def check_user_input(self): + """检查用户输入（Windows兼容）""" + if sys.platform == 'win32': try: - ready, _, _ = select.select([sys.stdin], [], [], timeout) - if ready: + import msvcrt + if msvcrt.kbhit(): user_input = sys.stdin.readline().strip().lower() - if user_input == 'r': - self.reset_robot() - elif user_input in ["sin", "random", "stop"]: - self.current_mode = user_input - print(f"\n🔄 运动模式已切换为：{user_input}") - elif user_input == 'q': - sim_running = False - print("\n📤 收到退出指令，仿真将结束...") - 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 + return user_input + except: + return None + return None + + def process_user_input(self, user_input): + """处理用户输入指令""" + if not user_input: + return - print("\n🔌 输入监听线程已优雅退出") + if user_input == 'r': + self.reset_robot() + elif user_input in ["walk", "wave", "sin", "random", "stop"]: + self.current_mode = user_input + print(f"\n🔄 运动模式已切换为：{user_input}") + if user_input == "walk": + print("👣 行走模式：机器人将进行自然行走动作") + elif user_input == "wave": + print("✋ 挥手模式：机器人将交替挥动手臂") + elif user_input == 'q': + global sim_running + sim_running = False + print("\n📤 收到退出指令，仿真将结束...") + 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(" - walk：行走模式（新增）") + print(" - wave：挥手模式（新增）") + print(" - sin：正弦运动模式") + print(" - random：随机运动模式") + print(" - stop：停止运动") + print(" - clear：清空绘图数据") + print(" - q：退出仿真") def run_simulation(self): """运行仿真主循环""" + # 加载模型 self.load_model() # 初始化绘图 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可视化 @@ -408,18 +524,27 @@ def run_simulation(self): # 打印操作提示 print("\n📌 仿真操作提示：") print(" - 输入 'r' 回车：重置机器人") - print(" - 输入 'sin'/'random'/'stop' 回车：切换运动模式") + print(" - 输入 'walk' 回车：行走模式（新增）") + print(" - 输入 'wave' 回车：挥手模式（新增）") + print(" - 输入 'sin' 回车：正弦运动模式") + print(" - 输入 'random' 回车：随机运动模式") + print(" - 输入 'stop' 回车：停止运动") print(" - 输入 'clear' 回车：清空绘图数据") print(" - 输入 'q' 回车：退出仿真") print(" - 按 Ctrl+C：强制退出仿真") - print("\n🚀 仿真开始...") + print("\n🚀 仿真开始（默认模式：行走）...") # 仿真主循环 - global sim_running last_step_time = time.perf_counter() while sim_running and v.is_running(): current_time = time.perf_counter() + + # 检查并处理用户输入 + user_input = self.check_user_input() + if user_input: + self.process_user_input(user_input) + if current_time - last_step_time >= self.config.timestep: # 更新关节控制 self.update_joint_controls() @@ -445,28 +570,45 @@ def run_simulation(self): # 处理matplotlib事件 plt.pause(0.001) - # 停止输入监听线程 - self.input_thread_running = False - input_thread.join(timeout=1.0) - - # 关闭绘图窗口 + # 清理资源 plt.close(self.fig) - print("\n🏁 仿真结束！") # ====================== 程序入口 ====================== if __name__ == "__main__": - # 设置matplotlib后端（避免显示问题） + # 设置matplotlib后端 import matplotlib matplotlib.use('TkAgg') + # Windows控制台编码修复 + if sys.platform == 'win32': + try: + # 设置控制台编码为UTF-8 + import subprocess + + subprocess.call('chcp 65001', shell=True) + except: + pass + # 初始化配置 config = SimConfig() # 创建仿真器并运行 simulator = HumanoidSimulator(config) - simulator.run_simulation() - sys.exit(0) \ No newline at end of file + try: + simulator.run_simulation() + except KeyboardInterrupt: + sim_running = False + print("\n⚠️ 用户中断，正在退出...") + except Exception as e: + print(f"\n❌ 程序异常：{e}") + import traceback + + traceback.print_exc() + finally: + # 确保资源正确释放 + plt.close('all') + sys.exit(0) \ No newline at end of file From aef828dc8629eeac36e2ce43e7d3faf1d59d04a1 Mon Sep 17 00:00:00 2001 From: oyzw2302 Date: Fri, 26 Dec 2025 16:25:17 +0800 Subject: [PATCH 11/12] =?UTF-8?q?=E4=BB=A3=E7=A0=81=E7=BB=93=E6=9E=84?= =?UTF-8?q?=E4=BC=98=E5=8C=96=20=E6=80=A7=E8=83=BD=E4=BC=98=E5=8C=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../humanoid_simulation.py | 710 ++++++++++-------- 1 file changed, 392 insertions(+), 318 deletions(-) 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 = """ - -