Mujoco学习记录

1. 可视化

• 设置手动控制仿真

1.手动调用mj_step推进仿真
2.调用sync()更新视图
3.循环上述过程

# 获得viewer所有控制接口的对象
self.handle = mujoco.viewer.launch_passive(self.model, self.data)

# 检查 viewer 是否仍在运行
while self.is_running():
    mujoco.mj_forward(self.model, self.data) # 计算动力学，同步仿真系统参数
    mujoco.mj_step(self.model, self.data) # 手动推进仿真一步
    self.sync() # 同步视图
    time.sleep(self.model.opt.timestep) # # 控制仿真速度

• 获取body的id，id是按照body的顺序

mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_BODY, 'right_finger')

• 通过id获取空间坐标系位置和位姿

end_effector_pos = self.data.body(self.end_effector_id).xpos,self.data.body(self.end_effector_id).xquat

 2. 场景渲染参数

    cam = mujoco.MjvCamera() # 创建MjvCamera对象，用于存储相机的参数
    opt = mujoco.MjvOption() # 创建MjvOption对象，用于配置渲染选项（是否显示关节、碰撞体、力线等）
    mujoco.mjv_defaultCamera(cam) # 为相机对象cam设置默认参数
    mujoco.mjv_defaultOption(opt) # 为渲染选项opt设置默认值
    pert = mujoco.MjvPerturb() # 创建MjvPerturb对象，用于处理用户与仿真场景的交互（鼠标拖动物体、施加外力等）
    con = mujoco.MjrContext(model, mujoco.mjtFontScale.mjFONTSCALE_150.value) # 创建一个MjrContext对象，设置缩放比例
    scene = mujoco.MjvScene(model, maxgeom=10000) # 指定场景可容纳的最大几何元素数量

# 更新渲染场景
    viewport = mujoco.MjrRect(0, 0, 1200, 900) # 创建一个视口对象，定义渲染窗口的尺寸和位置
    mujoco.mjv_updateScene(model, data, opt, pert, cam, mujoco.mjtCatBit.mjCAT_ALL.value, scene) # 更新场景渲染数据
    mujoco.mjr_render(viewport, scene, con) # 将更新后的场景数据渲染到屏幕上，生成可视化图像

• 关节角度

data.qpos[:7] = new_q

• mj_forward的作用：更新data全局参数，例如下面的复制qpos，mj_forward会更新xpos位置

data.qpos[:7] = new_q
print("data1",data.xpos)
mujoco.mj_forward(model, data)
print("data",data.xpos)

3. 键盘交互

右侧Alt是alt_gr，不是alt_r

if key_states[keyboard.Key.up]:
    data.qpos[0] += self.pos
if key_states[keyboard.Key.down]:
    data.qpos[0] -= self.pos
if key_states[keyboard.Key.left]:
    data.qpos[1] += self.pos
if key_states[keyboard.Key.right]:
    data.qpos[1] -= self.pos
if key_states[keyboard.Key.alt_l]:
    data.qpos[2] += self.pos
if key_states[keyboard.Key.alt_gr]:
    data.qpos[2] -= self.pos

• 关闭Mujoco重力

model.opt.gravity = [0, 0, 0]

4. 修改末端结构

• 需要修改mesh路径、joint旋转轴和运动范围、body、执行器等

5. xml文件元素

• <mesh> 定义机械臂各部件的三维几何形状，通过file引用外部模型文件（如.stl用于实体结构，.obj用于精细外观）

6. 夹具使用肌腱tendon

• 肌腱本身不产生力，需要通过<actuator>（执行器）提供驱动力。通过 “固定肌腱 + 等比例分配” 的方式，实现了左右手指关节的强制同步运动，是机械臂末端执行器（手爪）精准抓取的核心机制之一

  <actuator>
    <position class="Jaw" name="Jaw" joint="Jaw"/>
    <!-- 添加一个执行器，指定跟腱为split -->
    <general name="actuator8" tendon="split" forcerange="-35 35" ctrlrange="-30 30"
      gainprm="0.01568627451 0 0" biasprm="0 -100 -10"/>
  </actuator>

7. 关键帧Keyframe

• 预设指定状态，调用name即可重置该状态

<keyframe>
    <key name="home" qpos="0 -1.57079 1.57079 1.57079 -1.57079 -0.7853 0.04 0.04" ctrl="0 -1.57079 1.57079 1.57079 -1.57079 -1.57079 0"/>
</keyframe>

8. 接触信息data.contact

• 获取碰撞体的id等信息

# ncon表示当前仿真步中检测到的接触对总数（即有多少对几何体正在接触）
for i in range(data.ncon):
    # data.contact是一个数组，存储了所有接触对信息（如接触点坐标、法向量、接触力等）
    contact = data.contact[i]
    # 获取几何体对应的body_id
    # contact.geom1和contact.geom2是该接触对中两个几何体的标识符，就是2个几何体在模型中的索引值
    body1_id = model.geom_bodyid[contact.geom1]
    body2_id = model.geom_bodyid[contact.geom2]

9. 求关节角速度

# 通过当前位置与上一时刻位置的差值除以时间步长
calc_qvel = (self.last_qpos - self.q_vec[:7, self.index]) / self.model.opt.timestep
self.data.qpos[:7] = self.q_vec[:7, self.index]
self.last_qpos = self.data.qpos[:7]
print(f"qpos:{self.data.qpos[:7]}")
print(f"qvel:{self.data.qvel[:7]}")
print(f"calc_qvel:{calc_qvel}")
print(self.model.nv, self.model.nq)
self.joint_velocities[self.index] = self.data.qvel[:7]
self.index += 1
if self.index >= self.q_vec.shape[1]:
    self.index = 0
    time_steps = np.arange(self.q_vec.shape[1])
    fig, axes = plt.subplots(7, 1, figsize=(10, 10))
    for j in range(7):
        axes[j].plot(time_steps, self.joint_velocities[:, j], label=f'Joint {j + 1}', color=f'C{j}')
        axes[j].set_ylabel('Angular Velocity (rad/s)')
        axes[j].set_title(f'Joint {j + 1} Angular Velocity')
        axes[j].grid(True)
        axes[j].legend()
    plt.tight_layout()
    plt.show()

10. pinocchio安装

先激活conda环境，执行安装指令

conda install -c conda-forge pinocchio

代码片段	核心功能	关键说明
model = pin.buildModelFromUrdf(urdf_path)	加载 URDF 模型	将 URDF 文件解析为 Pinocchio 的Model对象（包含机器人的关节、连杆、惯性等静态结构信息）
data = model.createData()	创建运动学 / 动力学数据容器	Data对象用于存储动态计算结果（如正运动学后的位姿、雅克比矩阵等），每次计算前需重新创建或重置
lower_limits/upper_limits	获取关节限位	从Model中读取 URDF 定义的关节上下限（对应 URDF 中<limit lower="xx" upper="xx"/>）
model.nq / model.nv	配置 / 速度维度	nq是关节配置向量维度（Franka Panda 手臂有 7 个旋转关节，故nq=7）；nv是速度向量维度（旋转关节nv=nq，故nv=7）
pin.framesForwardKinematics(...)	计算正运动学	给定关节角度q（此处为全 0，即 “零位姿态”），计算所有连杆（Frame）的位姿（平移 + 旋转）
遍历model.joints/jointPlacements/inertias	打印机器人参数	分别输出关节名称、关节相对于父连杆的位姿、连杆的惯性参数（质量、质心、惯性矩阵）

10.1

oMdes = pinocchio.SE3(target_dir, np.array(target_pos))
# 用 “旋转矩阵（姿态）” 和 “3D 向量（位置）”，创建一个 “世界坐标系下的期望刚体位姿” 对象
oMdes 是 Pinocchio 中描述位姿的约定俗成命名，字母含义如下：
    o：表示 “参考坐标系” 是 世界坐标系（Origin Frame）；
    M：表示 “位姿（Motion）”，即 SE3 类型的位姿对象；
    des：表示 “期望（Desired）”，即这个位姿是我们希望刚体达到的目标。

不要手动写 3×3 矩阵（容易出错），建议用 Pinocchio 提供的工具函数从 “欧拉角”“四元数” 等更直观的方式转换