Как получить свойства geometri c тел в 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 файл фиксированными соединениями, и, чтобы все не упало, я приварил землю к каркасу мира.

Все препятствия - это коробки, и мне нужно извлечь координаты вершин ящиков . Мой план состоял в том, чтобы получить свойства width, depth и height каждой из фигур Box, в дополнение к их исходным точкам, и на основе этого вычислить вершины (при условии, что ни один из прямоугольников не повернут). До сих пор я пробовал использовать plant.GetBodiesWeldedTo(), а затем plant.GetCollisionGeometriesForBody(), но мне не удалось извлечь нужные мне свойства.

Как можно go получить вершины (или положение происхождение и ширина, глубина и высота) объектов после их импорта в Drake?

Answer 1

Изучить геометрию непросто. Это требует «овеществления формы». По сути, у вас есть доступ к Shape, но чтобы узнать, что это за тип c Shape, вы запускаете его через ShapeReifier. Пример этого можно найти в ShapeName, который принимает общую форму c и возвращает ее имя. Вам нужно что-то для извлечения размеров коробки.

Для извлечения позы вам также понадобится доступ к QueryObject из SceneGraph. Для этого вам необходимо оценить выходной порт SceneGraph с соответствующим Context. Я не собираюсь сосредотачиваться на получении из QueryObject (при необходимости мы можем открыть новый вопрос о переполнении стека).

Это многоэтапная вещь. Мы предполагаем, что вы начали с 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 не подходит для этого вопроса, но я все же хотел бы дополнить ответ Шона API Python;)

Вот пример выполнения SceneGraph -only самоанализ формы:

• Неофициальный пример (для базового использования c Rviz): drake_ros1_hacks/_ros_geometry.py - включает версию Python Reifier (if type(shape) == ...)

По строкам выполнения сопоставления между SceneGraph и MultibodyPlant в Python:

• Drake unittest: plant_test.py, MBP < -> SG запросы
• Учебники по Drake: Рендеринг MultibodyPlant, создание меток рендеринга