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 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():