1. 搭建具身智能体基础仿真环境

任务目标

在MuJoCo中构建一个包含机械臂与简单物体的仿真环境，实现关节角度、关节速度、末端执行器位姿以及视觉图像的读取，为后续具身智能体策略开发提供可交互的仿真基础。

环境依赖

• Python 3.9+
• MuJoCo 2.3.7+（pip install mujoco）
• NumPy（pip install numpy）
• OpenCV（仅用于图像保存验证，pip install opencv-python）

仿真环境XML定义

以下XML定义了一个6自由度关节臂（基座+5个旋转关节+夹爪简化体）以及一个待抓取的红色方块物体。场景包含基础光照与地面，相机固定在第三视角用于视觉观测。

<!-- robot_env.xml -->
<mujoco model="embodied_arm_env">
  <compiler angle="degree" coordinate="local"/>

  <option timestep="0.002" gravity="0 0 -9.81"/>

  <asset>
    <texture name="grid" type="2d" builtin="checker" width="512" height="512" rgb1="0.9 0.9 0.9" rgb2="0.7 0.7 0.7"/>
    <material name="ground_mat" texture="grid" texrepeat="5 5"/>
    <material name="arm_mat" rgba="0.3 0.5 0.9 1"/>
    <material name="obj_mat" rgba="0.9 0.2 0.2 1"/>
  </asset>

  <worldbody>
    <light pos="0 0 3" dir="0 0 -1"/>
    <geom name="ground" type="plane" size="2 2 0.1" material="ground_mat"/>

    <body name="base" pos="0 0 0.1">
      <geom name="base_geom" type="cylinder" size="0.08 0.05" pos="0 0 0.05" material="arm_mat"/>
      <joint name="joint1" type="hinge" axis="0 0 1" pos="0 0 0.1"/>

      <body name="link1" pos="0 0 0.1">
        <geom name="link1_geom" type="box" size="0.04 0.04 0.2" pos="0 0 0.2" material="arm_mat"/>
        <joint name="joint2" type="hinge" axis="0 1 0" pos="0 0 0.4"/>

        <body name="link2" pos="0 0 0.4">
          <geom name="link2_geom" type="box" size="0.03 0.03 0.18" pos="0 0 0.18" material="arm_mat"/>
          <joint name="joint3" type="hinge" axis="0 1 0" pos="0 0 0.36"/>

          <body name="link3" pos="0 0 0.36">
            <geom name="link3_geom" type="box" size="0.025 0.025 0.15" pos="0 0 0.15" material="arm_mat"/>
            <joint name="joint4" type="hinge" axis="0 1 0" pos="0 0 0.3"/>

            <body name="link4" pos="0 0 0.3">
              <geom name="link4_geom" type="box" size="0.02 0.02 0.1" pos="0 0 0.1" material="arm_mat"/>
              <joint name="joint5" type="hinge" axis="0 1 0" pos="0 0 0.2"/>

              <body name="gripper" pos="0 0 0.2">
                <geom name="gripper_geom" type="box" size="0.02 0.03 0.03" pos="0 0 0.03" material="arm_mat"/>
              </body>
            </body>
          </body>
        </body>
      </body>
    </body>

    <body name="target_box" pos="0.4 0.3 0.05">
      <geom name="box_geom" type="box" size="0.05 0.05 0.05" material="obj_mat"/>
      <freejoint name="box_free"/>
    </body>

    <camera name="cam_third" pos="0.8 -0.8 0.6" xyaxes="0.7 0.7 0 -0.4 0.4 0.8" mode="fixed"/>
  </worldbody>

  <sensor>
    <jointpos name="joint_pos" joint="joint1"/>
    <jointpos name="joint_pos2" joint="joint2"/>
    <jointpos name="joint_pos3" joint="joint3"/>
    <jointpos name="joint_pos4" joint="joint4"/>
    <jointpos name="joint_pos5" joint="joint5"/>
    <jointvel name="joint_vel1" joint="joint1"/>
    <jointvel name="joint_vel2" joint="joint2"/>
    <jointvel name="joint_vel3" joint="joint3"/>
    <jointvel name="joint_vel4" joint="joint4"/>
    <jointvel name="joint_vel5" joint="joint5"/>
    <framepos name="ee_pos" body="gripper"/>
    <framequat name="ee_quat" body="gripper"/>
  </sensor>
</mujoco>

Python数据读取与验证代码

import mujoco
import numpy as np
import cv2
import os

# 加载模型
xml_path = os.path.join(os.path.dirname(__file__), "robot_env.xml")
model = mujoco.MjModel.from_xml_path(xml_path)
data = mujoco.MjData(model)

# 重置仿真状态
mujoco.mj_resetData(model, data)

# 设置初始关节角度（单位：弧度）
data.qpos[0:5] = np.array([0.0, 0.5, -0.8, 0.3, 0.0])
mujoco.mj_forward(model, data)

# 传感器名称索引映射
sensor_names = [
    "joint_pos", "joint_pos2", "joint_pos3", "joint_pos4", "joint_pos5",
    "joint_vel1", "joint_vel2", "joint_vel3", "joint_vel4", "joint_vel5",
    "ee_pos", "ee_quat"
]
sensor_ids = {name: mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SENSOR, name) for name in sensor_names}
sensor_adr = {name: model.sensor_adr[sensor_ids[name]] for name in sensor_names}

# 读取关节位置与速度
joint_pos = data.sensordata[sensor_adr["joint_pos"]:sensor_adr["joint_pos"]+5]
joint_vel = data.sensordata[sensor_adr["joint_vel1"]:sensor_adr["joint_vel1"]+5]

# 读取末端执行器位置与四元数
ee_pos = data.sensordata[sensor_adr["ee_pos"]:sensor_adr["ee_pos"]+3]
ee_quat = data.sensordata[sensor_adr["ee_quat"]:sensor_adr["ee_quat"]+4]

# 渲染视觉图像（640x480 RGB）
renderer = mujoco.Renderer(model, 640, 480)
renderer.update_scene(data, camera="cam_third")
img = renderer.render()
renderer.close()

# 保存图像用于人工验证
cv2.imwrite("sim_view.png", cv2.cvtColor(img, cv2.COLOR_RGB2BGR))

# 打印关键数据
print("关节角度 (rad):", np.round(joint_pos, 4))
print("关节速度 (rad/s):", np.round(joint_vel, 4))
print("末端位置 (m):", np.round(ee_pos, 4))
print("末端四元数:", np.round(ee_quat, 4))
print("图像尺寸:", img.shape)

运行方式

# 将上述XML保存为 robot_env.xml，Python脚本保存为 read_sensors.py
# 在相同目录下执行：
python read_sensors.py

验证方法与通过条件

• 数据合理性：程序输出的关节角度应等于代码中设定的初始值 [0, 0.5, -0.8, 0.3, 0]（弧度），关节速度在初始静止状态下应接近0（数值噪声允许在1e-6量级）。
• 末端位姿：末端位置应位于机械臂运动学可达空间内（大致在x≈0.3~0.5m，y≈0~0.2m，z≈0.5~0.8m范围），四元数模长应等于1.0（误差小于1e-3）。
• 视觉输出：当前目录下生成 sim_view.png，打开后应能看到红色方块物体、蓝色机械臂以及棋盘格地面，三者清晰可见且无渲染错误。
• 仿真推进：在脚本末尾追加 mujoco.mj_step(model, data) 循环执行100步后，物体应因重力轻微下落或保持静止（若与地面接触），机械臂保持初始位形不变，证明物理仿真正常推进。

常见问题

• 图像全黑：检查相机 mode="fixed" 是否指向场景内部，可调整 pos 与 xyaxes 参数使目标位于视野中心。
• 关节角度读取异常：确认XML中 jointpos 传感器名称与代码中索引顺序一致，且 mj_name2id 返回有效ID（非-1）。
• 物体穿透地面：将 target_box 的初始z坐标设为0.05（半边长），确保物体底面恰好接触地面。