init cyy
This commit is contained in:
231
mujoco_api_demo.py
Normal file
231
mujoco_api_demo.py
Normal file
@@ -0,0 +1,231 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
MuJoCo API 核心用法演示 - 关节信息读取与控制
|
||||
"""
|
||||
import mujoco
|
||||
import numpy as np
|
||||
|
||||
# 加载模型
|
||||
XML_PATH = "/home/8x54zj-m/unitree_mujoco/data/go1/xml/go1.xml"
|
||||
MESH_PATH = "/home/8x54zj-m/unitree_mujoco/data/go1/meshes"
|
||||
|
||||
with open(XML_PATH, 'r') as f:
|
||||
xml = f.read()
|
||||
xml = xml.replace('meshdir="../meshes/"', f'meshdir="{MESH_PATH}"')
|
||||
xml = xml.replace('<default>\n=', '<default>\n')
|
||||
|
||||
model = mujoco.MjModel.from_xml_string(xml)
|
||||
data = mujoco.MjData(model)
|
||||
|
||||
# ============================================================
|
||||
# 1. 获取模型基本信息
|
||||
# ============================================================
|
||||
print("=" * 60)
|
||||
print("1. 模型基本信息")
|
||||
print("=" * 60)
|
||||
print(f" nq (自由度数量): {model.nq}") # 位置自由度
|
||||
print(f" nv (速度自由度): {model.nv}") # 速度自由度
|
||||
print(f" nu (控制数量): {model.nu}") # actuator 数量
|
||||
print(f" njnt (关节数量): {model.njnt}") # 关节数量
|
||||
|
||||
# ============================================================
|
||||
# 2. 读取关节信息
|
||||
# ============================================================
|
||||
print("\n" + "=" * 60)
|
||||
print("2. 关节信息")
|
||||
print("=" * 60)
|
||||
|
||||
# 关节索引 0 是 world 或 freejoint, 从 1 开始是实际关节
|
||||
print("\n关节列表 (用 mj_id2name):")
|
||||
for i in range(model.njnt):
|
||||
name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_JOINT, i)
|
||||
print(f" [{i:2d}] {name}")
|
||||
|
||||
# ============================================================
|
||||
# 3. 状态向量详解
|
||||
# ============================================================
|
||||
print("\n" + "=" * 60)
|
||||
print("3. 状态向量")
|
||||
print("=" * 60)
|
||||
|
||||
print(f"""
|
||||
MuJoCo 状态向量布局 (Go1 是自由关节机器人):
|
||||
|
||||
qpos (位置, {model.nq}维):
|
||||
qpos[0:3] = 全局位置 (x, y, z)
|
||||
qpos[3:7] = 全局四元数 (qx, qy, qz, qw)
|
||||
qpos[7:19] = 12 个关节位置 (hip, thigh, calf)
|
||||
qpos[19:] = 额外数据
|
||||
|
||||
qvel (速度, {model.nv}维):
|
||||
qvel[0:3] = 全局线速度
|
||||
qvel[3:6] = 全局角速度
|
||||
qvel[6:18] = 12 个关节速度
|
||||
""")
|
||||
|
||||
print("当前状态:")
|
||||
print(f" qpos = {data.qpos}")
|
||||
print(f" qvel = {data.qvel}")
|
||||
|
||||
# ============================================================
|
||||
# 4. 通过关节名读取特定关节数据
|
||||
# ============================================================
|
||||
print("\n" + "=" * 60)
|
||||
print("4. 通过关节名读取数据")
|
||||
print("=" * 60)
|
||||
|
||||
# 方法: mj_name2id 获取关节 ID, 然后用 jnt_qposadr/jnt_dofadr 找到索引
|
||||
def get_joint_state(model, data, joint_name):
|
||||
"""获取指定关节的位置和速度
|
||||
|
||||
注意: MuJoCo 中:
|
||||
- qpos 位置索引通过 jnt_qposadr 获取
|
||||
- qvel 速度索引通过 jnt_dofadr 获取 (不是 jnt_qveladr!)
|
||||
"""
|
||||
joint_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, joint_name)
|
||||
if joint_id < 0:
|
||||
return None, None
|
||||
qposadr = model.jnt_qposadr[joint_id]
|
||||
dofadr = model.jnt_dofadr[joint_id] # 注意: 是 dofadr 不是 qveladr
|
||||
# 读取位置 (每个关节 1 个 qpos 值, freejoint 是 7 个)
|
||||
nq = model.jnt_type[joint_id] == mujoco.mjtJoint.mjJNT_FREE and 7 or 1
|
||||
pos = data.qpos[qposadr:qposadr+nq]
|
||||
# 读取速度 (每个关节 1 个 dof, freejoint 没有速度直接索引)
|
||||
vel = data.qvel[dofadr] if dofadr >= 0 else 0.0
|
||||
return pos, vel
|
||||
|
||||
# 示例: 读取前腿关节
|
||||
print("\n读取前右腿关节:")
|
||||
for name in ["FR_hip_joint", "FR_thigh_joint", "FR_calf_joint"]:
|
||||
pos, vel = get_joint_state(model, data, name)
|
||||
pos_str = f"{pos:.4f}" if np.isscalar(pos) else str(pos)
|
||||
print(f" {name:20s}: pos={pos_str}, vel={vel:.4f}")
|
||||
|
||||
# ============================================================
|
||||
# 5. 控制输出
|
||||
# ============================================================
|
||||
print("\n" + "=" * 60)
|
||||
print("5. 控制输出 (ctrl)")
|
||||
print("=" * 60)
|
||||
|
||||
print(f"\n控制向量长度: {model.nu}")
|
||||
print(f"控制范围 (从 XML 读取):\n{model.actuator_ctrlrange}")
|
||||
|
||||
# 遍历所有 actuator
|
||||
print("\nActuator 列表:")
|
||||
for i in range(model.nu):
|
||||
name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_ACTUATOR, i)
|
||||
# 获取 actuator 关联的 joint
|
||||
trn_joint = model.actuator_trnid[i, 0]
|
||||
joint_name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_JOINT, trn_joint) if trn_joint >= 0 else "none"
|
||||
ctrl_range = model.actuator_ctrlrange[i]
|
||||
print(f" [{i:2d}] {name:15s} -> {joint_name:15s} range=[{ctrl_range[0]:.2f}, {ctrl_range[1]:.2f}]")
|
||||
|
||||
# ============================================================
|
||||
# 6. 控制输出方法
|
||||
# ============================================================
|
||||
print("\n" + "=" * 60)
|
||||
print("6. 控制输出方法")
|
||||
print("=" * 60)
|
||||
|
||||
print("""
|
||||
# 方法1: 直接设置所有控制
|
||||
data.ctrl[:] = 0.0
|
||||
|
||||
# 方法2: 设置单个控制
|
||||
data.ctrl[0] = 1.0
|
||||
|
||||
# 方法3: 用索引设置 (与 actuator 顺序对应)
|
||||
data.ctrl[np.arange(12)] = target_positions
|
||||
|
||||
# 方法4: 先计算再设置
|
||||
target_torque = kp * (desired - actual) + kd * (desired_vel - actual_vel)
|
||||
data.ctrl[:] = target_torque
|
||||
""")
|
||||
|
||||
# 示例: 设置目标关节位置
|
||||
desired_joint_pos = np.zeros(12)
|
||||
for i in range(12):
|
||||
desired_joint_pos[i] = np.random.uniform(-0.5, 0.5)
|
||||
|
||||
print(f"\n示例: 设置目标关节位置: {desired_joint_pos}")
|
||||
data.ctrl[:] = desired_joint_pos
|
||||
print(f"已写入 ctrl: {data.ctrl}")
|
||||
|
||||
# ============================================================
|
||||
# 7. 仿真循环
|
||||
# ============================================================
|
||||
print("\n" + "=" * 60)
|
||||
print("7. 仿真循环示例")
|
||||
print("=" * 60)
|
||||
|
||||
print("""
|
||||
# 基本仿真循环:
|
||||
for _ in range(100):
|
||||
# 1. 读取当前状态
|
||||
current_pos = data.qpos[7:19] # 关节位置
|
||||
current_vel = data.qvel[6:18] # 关节速度
|
||||
|
||||
# 2. 计算控制量 (这里用简单的 PD 控制作为示例)
|
||||
desired_pos = np.array([...]) # 目标位置
|
||||
error = desired_pos - current_pos
|
||||
data.ctrl[:] = kp * error - kd * current_vel
|
||||
|
||||
# 3. 执行一步仿真
|
||||
mujoco.mj_step(model, data)
|
||||
|
||||
# 4. (可选) 渲染
|
||||
# renderer.update_scene(data)
|
||||
# img = renderer.render()
|
||||
""")
|
||||
|
||||
# 实际运行 50 步
|
||||
print("\n运行 50 步仿真 (零控制):")
|
||||
data.ctrl[:] = 0.0
|
||||
for step in range(50):
|
||||
mujoco.mj_step(model, data)
|
||||
if step % 10 == 0:
|
||||
print(f" Step {step:3d}: trunk_z={data.qpos[2]:.4f}m, "
|
||||
f"FR_hip={data.qpos[7]:.4f}, FR_thigh={data.qpos[8]:.4f}")
|
||||
|
||||
print("\n机器人倒下是因为零控制 + 重力!")
|
||||
print("真实 RL 控制需要在每步根据策略计算正确的 ctrl 值。")
|
||||
|
||||
# ============================================================
|
||||
# 8. 重要 API 速查
|
||||
# ============================================================
|
||||
print("\n" + "=" * 60)
|
||||
print("8. 重要 API 速查")
|
||||
print("=" * 60)
|
||||
print("""
|
||||
┌─────────────────────────────────────────────────────────────┐
|
||||
│ 读取模型信息 │
|
||||
├─────────────────────────────────────────────────────────────┤
|
||||
│ model.nq # 自由度数 │
|
||||
│ model.nu # 控制数 │
|
||||
│ model.njnt # 关节数 │
|
||||
│ mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_JOINT, i) │
|
||||
│ mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, name) │
|
||||
├─────────────────────────────────────────────────────────────┤
|
||||
│ 读取状态 │
|
||||
├─────────────────────────────────────────────────────────────┤
|
||||
│ data.qpos # 位置向量 │
|
||||
│ data.qvel # 速度向量 │
|
||||
│ data.qacc # 加速度向量 │
|
||||
│ data.ctrl # 控制向量 (要设置的) │
|
||||
├─────────────────────────────────────────────────────────────┤
|
||||
│ 关节层面 │
|
||||
├─────────────────────────────────────────────────────────────┤
|
||||
│ joint_id = mujoco.mj_name2id(model, ..., joint_name) │
|
||||
│ qpos_adr = model.jnt_qposadr[joint_id] │
|
||||
│ qvel_adr = model.jnt_qveladr[joint_id] │
|
||||
│ joint_pos = data.qpos[qpos_adr] │
|
||||
│ joint_vel = data.qvel[qvel_adr] │
|
||||
├─────────────────────────────────────────────────────────────┤
|
||||
│ 仿真 │
|
||||
├─────────────────────────────────────────────────────────────┤
|
||||
│ mujoco.mj_step(model, data) # 执行一步 │
|
||||
│ mujoco.mj_stepN(model, data, N) # 执行 N 步 │
|
||||
│ mujoco.mj_resetData(model, data) # 重置状态 │
|
||||
└─────────────────────────────────────────────────────────────┘
|
||||
""")
|
||||
Reference in New Issue
Block a user