如何获取凸体的轴对齐包围盒（AABB）以实现与地面贴合
2026-8-4
获取Drake中凸体的轴对齐包围盒（AABB）
要获取你添加的凸体的AABB并调整其位置与地面齐平，按以下步骤操作：
1. 确保SceneGraph关联与Plant初始化完成
首先确认代码中已关联SceneGraph（Drake处理几何查询的核心组件），并在添加完所有刚体和几何后完成Plant的初始化：
from pydrake.geometry import SceneGraph # 初始化阶段关联SceneGraph scene_graph = SceneGraph() self.plant.RegisterAsSourceForSceneGraph(scene_graph) # 在添加完所有凸体、刚体之后执行 self.plant.Finalize()
2. 创建上下文并获取几何查询对象
Drake的几何计算依赖上下文（Context），需要创建Plant的默认上下文，并获取用于几何查询的QueryObject：
# 创建Plant默认上下文 plant_context = self.plant.CreateDefaultContext() # 从根上下文获取SceneGraph的上下文 scene_graph_context = scene_graph.GetMyContextFromRoot(plant_context) # 获取几何查询对象 query_object = scene_graph.get_query_output_port().Eval(scene_graph_context)
3. 计算凸体的AABB
你可以通过之前保存的geom_id直接计算该凸体在世界坐标系下的AABB：
# 计算世界坐标系下的AABB world_aabb = query_object.ComputeAabb(geom_id) # 获取世界坐标系下的最低点z值 world_min_z = world_aabb.z_min()
如果需要获取凸体自身局部坐标系下的AABB（用于后续调整刚体初始位姿更方便），可以通过SceneGraph的检查器获取原始形状数据：
# 获取几何形状的检查器 inspector = scene_graph.inspector() # 获取对应geom_id的形状 convex_shape = inspector.GetShape(geom_id) # 生成局部坐标系下的AABB local_aabb = convex_shape.MakeAabb() # 获取局部坐标系下的最低点z值 local_min_z = local_aabb.z_min()
4. 调整凸体位置与地面齐平
如果地面是z=0的平面，要让凸体最低点与地面齐平，只需将刚体的初始z坐标设置为-local_min_z（抵消局部坐标系下最低点的偏移）：
# 调整初始位姿的z分量，使凸体最低点对齐z=0地面 H = RigidTransform(RotationMatrix(np.eye(3)), np.array([0., 0., -local_min_z])) # 用调整后的H添加刚体和几何（替换你原代码中的H）
内容的提问来源于stack exchange，提问作者HoliInn
相关产品推荐
相关产品推荐
