mujoco获取所有部件的质心位置

import mujoco

# 加载模型和数据
model = mujoco.MjModel.from_xml_path('path/to/your/model.xml')
data = mujoco.MjData(model)

# 更新模型状态
mujoco.mj_step(model, data)

# 计算质心位置
mujoco.mj_comPos(model, data)

# 获取质心位置
subtree_com = data.subtree_com.reshape(-1, 3)

# 打印所有部件的质心位置
for i in range(model.nbody):
    print(f"Body {i} (name: {model.body(i).name}) - Center of Mass: {subtree_com[i]}")