MuJoCo中几何体轴对齐包围盒(AABB)的坐标系解析

2025-05-25 08:37:14作者：凤尚柏Louis

概述

在MuJoCo物理引擎中，几何体(geom)的轴对齐包围盒(AABB)是一个重要的概念，它用于快速检测碰撞和进行空间查询。本文将深入解析MuJoCo中AABB数据的坐标系含义及其计算方法。

AABB数据结构

MuJoCo中的geom_aabb属性返回一个包含两个部分的数组：

中心点坐标(center)
包围盒尺寸(size)

每个几何体的AABB数据由6个浮点数组成，前3个表示中心点坐标，后3个表示包围盒在各个轴上的半边长。

坐标系解析

关键点在于理解AABB数据的参考坐标系。根据MuJoCo的官方实现：

AABB数据是相对于几何体自身坐标系的，而不是世界坐标系或父物体(body)坐标系
几何体自身坐标系的原点位于data->geom_xpos位置
几何体的姿态(旋转)由data->geom_xmat决定

这意味着当我们获取geom_aabb数据时：

中心点坐标是相对于几何体自身坐标系的
要获取世界坐标系下的AABB，需要考虑几何体的位置和姿态

实际应用示例

以一个由上下两个方块组成的物体为例：

<body name="object">
    <geom name="top" type="box" density="500" size=".16 .32 .32" pos=".0 .0 .32"/>
    <geom name="bottom" type="box" density="2000" size=".16 .32 .32" pos=".0 .0 -.32"/>
</body>

在这个例子中：

两个几何体在自身坐标系中的AABB中心都是(0,0,0)
尺寸都是(0.16,0.32,0.32)
实际位置由几何体相对于父物体的偏移(pos属性)和父物体的位置共同决定

计算世界坐标系下的AABB

要计算几何体在世界坐标系下的完整AABB，可以按照以下步骤：

获取几何体的位置和姿态
将局部AABB的8个顶点转换到世界坐标系
找出这些顶点在各个轴上的最小/最大值

Python实现示例：

# 假设m是模型对象
max_point = None
min_point = None

for geom_id in range(m.ngeom):
    # 获取几何体位置和姿态
    pos = m.geom_pos[geom_id]
    mat = m.geom_xmat[geom_id].reshape(3,3)
    
    # 获取AABB数据
    aabb = m.geom_aabb[geom_id]
    local_center, half_size = aabb[:3], aabb[3:]
    
    # 生成局部AABB的8个顶点
    for i in (-1, 1):
        for j in (-1, 1):
            for k in (-1, 1):
                # 局部坐标到世界坐标的转换
                local_point = half_size * np.array([i, j, k]) + local_center
                world_point = pos + mat @ local_point
                
                # 更新最大最小点
                if max_point is None or np.all(max_point <= world_point):
                    max_point = world_point
                if min_point is None or np.all(world_point <= min_point):
                    min_point = world_point

# 计算完整AABB尺寸
aabb_size = max_point - min_point