如何在 Drake 中获取物体的几何特性？

Question

我有一个指定一组障碍物的 .urdf 文件，我将其添加到我的模拟中，如下所示：

drake::systems::DiagramBuilder<double> builder;
auto [plant, scene_graph] =
    drake::multibody::AddMultibodyPlantSceneGraph(&builder, 0.0);

drake::multibody::Parser parser(&plant, &scene_graph);
auto obstacles = parser.AddModelFromFile("obstacles.urdf");
plant.WeldFrames(
        plant.world_frame(), plant.GetFrameByName("ground", obstacles));

所有障碍物都通过固定关节固定在 .urdf 文件中的 "ground" 对象上，为了防止一切掉落，我将地面焊接到世界框架上。

所有的障碍物都是盒子，我需要提取盒子的顶点坐标。我的计划是获取每个 Box 形状的 width、depth 和 height 属性，以及它们的起源，并据此计算顶点（假设 none 的框被旋转）。到目前为止，我已经尝试使用 plant.GetBodiesWeldedTo()，然后是 plant.GetCollisionGeometriesForBody()，但我无法提取我需要的属性。

将对象导入 Drake 后，如何获取对象的顶点（或原点位置以及宽度、深度和高度）？

Answer 1

内省几何并不简单。它需要 "shape reification"。本质上，您可以访问 Shape，但要找出它是什么特定类型的 Shape，您需要通过运行 ShapeReifier。可以在 ShapeName 中找到一个例子，它采用通用形状和 returns 它的名称。你想要的是提取框尺寸的东西。

要提取姿势，您还需要从 SceneGraph 访问 QueryObject。为此，您需要使用适当的 Context 评估 SceneGraph 的输出端口。我不会专注于获取 QueryObject（如有必要，我们可以就此提出一个新的 Whosebug 问题）。

这是一个多阶段的事情。我们假设您从 MultibodyPlant::GetCollisionGeometriesForBody...

开始

class BoxExtractor : public ShapeReifier {
 public:
  explicit BoxExtractor(const Shape& shape) { shape.Reify(this); }

  std::optional<Box> box() const { return box_; }

 private:
  void ImplementGeometry(const Box& box, void*) override { box_ = box; }
  void ImplementGeometry(const Capsule&, void*) override {}
  void ImplementGeometry(const Cylinder&, void*) override {}
  void ImplementGeometry(const Convex&, void*) override {}
  void ImplementGeometry(const Ellipsoid&, void*) override {}
  void ImplementGeometry(const HalfSpace&, void*) override {}
  void ImplementGeometry(const Mesh&, void*) override {}
  void ImplementGeometry(const Sphere&, void*) override {}

  std::optional<Box> box_{};
};

const QueryObject<double>& query_object = ....;  // got it somehow.
const auto& inspector = scene_graph.model_inspector();
const auto& collision_geometries = plant.GetCollisionGeometriesForBody(some_body);
for (const GeometryId id : collision_geometries) {
  std::optional<Box> box = BoxExtractor(inspector.GetShape(id)).box();
  if (box) {
    const RigidTransformd& X_WB = query_object.X_WG(id);
    const Vector3d v_WBx = X_WB.rotation().col(0);
    const Vector3d v_WBy = X_WB.rotation().col(1);
    const Vector3d v_WBz = X_WB.rotation().col(2);
    const Vector3d& p_WBo = X_WB.translation();
    vector<Vector3d> corners;
    const Vector3d half_size{box->width(), box->depth(), box->height()};
    for (const double x_sign : {-1., 1.}) {
      for (const double y_sign : {-1., 1.}) {
        for (const double z_sign : {-1., 1.}) {
          corners.emplace_back(x_sign * half_size(0) * v_WBx +
                               y_sign * half_size(1) * v_WBy +
                               z_sign * half_size(2) * v_WBz);
        }
      }
    }
    // Do stuff with the eight corners.
  }
}

（我大约 98% 确定代码会按原样工作...我是即时输入的，不能保证它会复制和粘贴。）

编辑 --

我意识到我只回答了你问题的一半。如何获得盒子尺寸。您仍然想知道方框的位置，这样您就可以计算方框的顶点。为此，我修改了示例代码。

Answer 2

我知道 Python 不是这个问题的语言，但我仍然想用 Python API ;)

来补充 Sean 的回答

这是一个仅进行 SceneGraph 形状内省的示例：

非官方示例（用于 Rviz 的基本用法）：drake_ros1_hacks/_ros_geometry.py - 包括 Python 版本的 reifier (if type(shape) == ...)

按照 Python 中 SceneGraph 和 MultibodyPlant 之间的映射的思路：

德雷克单元测试：plant_test.py, MBP <-> SG queries
德雷克教程：MultibodyPlant rendering, making rendering labels

如何在 Drake 中获取物体的几何特性？

How to get geometric properties of bodies in Drake?

drake