1. 仿真环境搭建与基础运动控制

import mujoco
import mujoco.viewer
import numpy as np
import time

# 四足机器人XML模型（简化版，含12个关节）
xml = """
<mujoco model="quadruped">
  <compiler angle="degree"/>
  <option timestep="0.002"/>
  <worldbody>
    <light diffuse="0 0 1" pos="0 0 3"/>
    <geom type="plane" size="2 2 0.1" pos="0 0 -0.1"/>
    <body name="torso" pos="0 0 0.3">
      <geom type="box" size="0.2 0.1 0.05" mass="5"/>
      <joint name="root" type="free"/>
      <!-- 前左腿 -->
      <body name="FL_hip" pos="0.15 0.1 0">
        <geom type="sphere" size="0.03" mass="0.5"/>
        <joint name="FL_hip_j" type="hinge" axis="0 1 0" range="-30 30"/>
        <body name="FL_thigh" pos="0 0 -0.1">
          <geom type="capsule" size="0.02" fromto="0 0 0 0 0 -0.15" mass="1"/>
          <joint name="FL_thigh_j" type="hinge" axis="0 1 0" range="-120 120"/>
          <body name="FL_foot" pos="0 0 -0.15">
            <geom type="sphere" size="0.02" mass="0.2"/>
            <joint name="FL_foot_j" type="hinge" axis="0 1 0" range="-60 60"/>
          </body>
        </body>
      </body>
      <!-- 前右腿 -->
      <body name="FR_hip" pos="0.15 -0.1 0">
        <geom type="sphere" size="0.03" mass="0.5"/>
        <joint name="FR_hip_j" type="hinge" axis="0 1 0" range="-30 30"/>
        <body name="FR_thigh" pos="0 0 -0.1">
          <geom type="capsule" size="0.02" fromto="0 0 0 0 0 -0.15" mass="1"/>
          <joint name="FR_thigh_j" type="hinge" axis="0 1 0" range="-120 120"/>
          <body name="FR_foot" pos="0 0 -0.15">
            <geom type="sphere" size="0.02" mass="0.2"/>
            <joint name="FR_foot_j" type="hinge" axis="0 1 0" range="-60 60"/>
          </body>
        </body>
      </body>
      <!-- 后左腿 -->
      <body name="RL_hip" pos="-0.15 0.1 0">
        <geom type="sphere" size="0.03" mass="0.5"/>
        <joint name="RL_hip_j" type="hinge" axis="0 1 0" range="-30 30"/>
        <body name="RL_thigh" pos="0 0 -0.1">
          <geom type="capsule" size="0.02" fromto="0 0 0 0 0 -0.15" mass="1"/>
          <joint name="RL_thigh_j" type="hinge" axis="0 1 0" range="-120 120"/>
          <body name="RL_foot" pos="0 0 -0.15">
            <geom type="sphere" size="0.02" mass="0.2"/>
            <joint name="RL_foot_j" type="hinge" axis="0 1 0" range="-60 60"/>
          </body>
        </body>
      </body>
      <!-- 后右腿 -->
      <body name="RR_hip" pos="-0.15 -0.1 0">
        <geom type="sphere" size="0.03" mass="0.5"/>
        <joint name="RR_hip_j" type="hinge" axis="0 1 0" range="-30 30"/>
        <body name="RR_thigh" pos="0 0 -0.1">
          <geom type="capsule" size="0.02" fromto="0 0 0 0 0 -0.15" mass="1"/>
          <joint name="RR_thigh_j" type="hinge" axis="0 1 0" range="-120 120"/>
          <body name="RR_foot" pos="0 0 -0.15">
            <geom type="sphere" size="0.02" mass="0.2"/>
            <joint name="RR_foot_j" type="hinge" axis="0 1 0" range="-60 60"/>
          </body>
        </body>
      </body>
    </body>
  </worldbody>
  <actuator>
    <position joint="FL_hip_j" kp="100" kv="5"/>
    <position joint="FL_thigh_j" kp="100" kv="5"/>
    <position joint="FL_foot_j" kp="100" kv="5"/>
    <position joint="FR_hip_j" kp="100" kv="5"/>
    <position joint="FR_thigh_j" kp="100" kv="5"/>
    <position joint="FR_foot_j" kp="100" kv="5"/>
    <position joint="RL_hip_j" kp="100" kv="5"/>
    <position joint="RL_thigh_j" kp="100" kv="5"/>
    <position joint="RL_foot_j" kp="100" kv="5"/>
    <position joint="RR_hip_j" kp="100" kv="5"/>
    <position joint="RR_thigh_j" kp="100" kv="5"/>
    <position joint="RR_foot_j" kp="100" kv="5"/>
  </actuator>
</mujoco>
"""

model = mujoco.MjModel.from_xml_string(xml)
data = mujoco.MjData(model)

# 站立姿态目标角度（度）：髋0°，大腿-45°（外展），小腿90°（弯曲）
stand_pose = {
    "FL_hip_j": 0, "FL_thigh_j": -45, "FL_foot_j": 90,
    "FR_hip_j": 0, "FR_thigh_j": -45, "FR_foot_j": 90,
    "RL_hip_j": 0, "RL_thigh_j": -45, "RL_foot_j": 90,
    "RR_hip_j": 0, "RR_thigh_j": -45, "RR_foot_j": 90,
}

# 设置初始位置控制目标
for name, angle in stand_pose.items():
    data.ctrl[model.actuator(name).id] = angle

# 仿真循环
with mujoco.viewer.launch_passive(model, data) as viewer:
    start = time.time()
    while time.time() - start < 10:  # 运行10秒
        mujoco.mj_step(model, data)
        viewer.sync()
        time.sleep(0.002)