如何在Plant中手动获取模型顶点、表面法线及本体注册帧下的场景几何体点位？

直接访问几何体原始数据（无需标准工作流） #

你可以绕过配对查询的方式，直接获取几何体关联的底层Shape对象，从中提取本体注册坐标系下的顶点、法线或采样点：

• 步骤1：定位目标几何体
  通过模型实例名称获取对应的几何体：

const auto& model_instance = plant.GetModelInstanceByName("你的模型实例名称");
const std::shared_ptr<const Shape>& geometry = model_instance->geometry();

• 步骤2：提取Mesh类型几何体的顶点与法线
  如果几何体是Mesh类型，其顶点和法线数据本身就基于本体注册坐标系存储：

if (const auto* mesh = dynamic_cast<const Mesh*>(geometry.get())) {
    // 本体坐标系下的顶点数组（每列对应一个顶点）
    const Eigen::MatrixXd& vertices_B = mesh->vertices();
    // 本体坐标系下的顶点法线数组
    const Eigen::MatrixXd& normals_B = mesh->vertex_normals();

    // 遍历示例：输出第一个顶点和法线
    Eigen::Vector3d p_B = vertices_B.col(0);
    Eigen::Vector3d n_B = normals_B.col(0);
}

• 步骤3：处理基础几何体（Box/Sphere等）
  对于非Mesh的基础几何体，可通过其参数生成本体坐标系下的点位：
  比如Box的8个顶点：

if (const auto* box = dynamic_cast<const Box*>(geometry.get())) {
    const double x_half = box->width() / 2.0;
    const double y_half = box->depth() / 2.0;
    const double z_half = box->height() / 2.0;

    std::vector<Eigen::Vector3d> box_vertices_B = {
        {-x_half, -y_half, -z_half},
        {x_half, -y_half, -z_half},
        {x_half, y_half, -z_half},
        {-x_half, y_half, -z_half},
        {-x_half, -y_half, z_half},
        {x_half, -y_half, z_half},
        {x_half, y_half, z_half},
        {-x_half, y_half, z_half}
    };
}

补充说明 #

你之前使用的ComputeSignedDistancePairwiseClosestPoints()是用于两个几何体间的距离配对查询，适合获取最近点而非原始顶点数据。直接访问Shape的底层数据才是获取本体坐标系点位/法线的直接途径，无需依赖工作流或配对计算。

内容的提问来源于stack exchange，提问作者Xiao