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