MuJoCo中接触力在不同坐标系间的转换方法

概述

在物理仿真引擎MuJoCo中，正确处理接触力在不同坐标系间的转换是一个常见需求。本文将详细介绍如何将接触力从接触坐标系转换到几何体局部坐标系的方法，这对于分析仿真中各个几何体受到的力非常重要。

坐标系转换基础

在MuJoCo中，每个几何体都有自己的局部坐标系，而接触发生时也会建立一个接触坐标系。我们需要理解这些坐标系之间的关系：

1. 几何体坐标系：由geom_xpos(位置)和geom_xmat(旋转矩阵)定义
2. 接触坐标系：由contact.frame(旋转矩阵)和接触位置定义

关键点在于理解MuJoCo中geom_xmat的性质：它是一个3×3的旋转矩阵，可以将局部坐标系的向量转换到世界坐标系。

接触力转换步骤

1. 获取接触力：使用mj_contactForce函数获取接触力，这个力是在接触坐标系中表示的
2. 接触坐标系到世界坐标系：将接触力从接触坐标系转换到世界坐标系
3. 世界坐标系到几何体坐标系：再将力从世界坐标系转换到目标几何体的局部坐标系

具体实现如下：

# 获取接触力(在接触坐标系中)
forcetorque = np.zeros(6)
mujoco.mj_contactForce(model, data, i, forcetorque)
con_force_i = forcetorque[0:3]

# 获取接触坐标系旋转矩阵(注意转置)
contact_xmat = con.frame.reshape(3,3).T

# 获取几何体旋转矩阵
geom_xmat = data.geom_xmat[geom_id].reshape(3,3)

# 转换到几何体坐标系
f_geom = geom_xmat.T @ contact_xmat @ con_force_i

重要注意事项

1. 力的方向：当转换到第二个几何体坐标系时，需要应用牛顿第三定律，对力取反
2. 矩阵转置：MuJoCo中contact.frame是列主序存储的，需要转置才能得到正确的旋转矩阵
3. 平移处理：对于力的转换不需要考虑平移，因为力是自由向量；只有点坐标转换才需要考虑平移

实际应用示例

假设我们需要分析两个碰撞几何体各自受到的力：

# 对每个接触进行处理
for i, con in enumerate(data.contact):
    # 获取接触力
    forcetorque = np.zeros(6)
    mujoco.mj_contactForce(model, data, i, forcetorque)
    con_force = forcetorque[0:3]
    
    # 获取接触坐标系
    contact_xmat = con.frame.reshape(3,3).T
    
    # 转换到geom1坐标系
    geom1_xmat = data.geom_xmat[con.geom1].reshape(3,3)
    f_geom1 = geom1_xmat.T @ contact_xmat @ con_force
    
    # 转换到geom2坐标系(注意力方向相反)
    geom2_xmat = data.geom_xmat[con.geom2].reshape(3,3)
    f_geom2 = geom2_xmat.T @ contact_xmat @ -con_force

总结

在MuJoCo仿真中正确处理接触力的坐标系转换需要注意旋转矩阵的方向性和力的向量特性。通过将接触力先转换到世界坐标系，再转换到目标几何体坐标系，可以准确分析各个几何体受到的力。这种方法保证了力的方向和大小在转换过程中保持不变，是物理仿真分析中的重要技术。