CODEWITHSUNDEEP
drake
bernhardpg
bernhardpg

Reputation: 61

How to get geometric properties of bodies in Drake?

I have a .urdf file specifying a group of obstacles, that I add to my simulation like this:

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));

Where all the obstacles are fixed to a the "ground" object in the .urdf file by fixed joints, and to stop everything from falling down I welded the ground to the world frame.

All the obstacles are boxes, and I need to extract the coordinates of the vertices of the boxes. My plan was to get the width, depth and height properties of each of the Box shapes, in addition to their origins, and from this calculate the vertices (assuming none of the boxes are rotated). So far I have tried using plant.GetBodiesWeldedTo() and then plant.GetCollisionGeometriesForBody(), but I have not been able to extract the properties that I need.

How would one go about getting the vertices (or the position of the origin and width, depth and height) of the objects, after importing them into Drake?

Upvotes: 1

Views: 524

Answers (2)

Eric Cousineau
Eric Cousineau

Reputation: 2004

I know Python isn't the language for this question, but I would still like to complement Sean's answer with the Python API ;)

Here's an example of doing SceneGraph-only shape introspection:

Along the lines of exercising the mapping between SceneGraph and MultibodyPlant in Python:

Upvotes: 0

Sean Curtis
Sean Curtis

Reputation: 1923

Introspecting geometry is not straightforward. It requires "shape reification". Essentially, you have access to Shape but to find out what specific type of Shape it is, you run it through a ShapeReifier. An example of that can be found in ShapeName which takes a generic shape and returns its name. What you want is something to extract box dimensions.

To extract pose, you'll also need access to a QueryObject from SceneGraph. To do that, you'll need to evaluate SceneGraph's output port with an appropriate Context. I'm not going to focus on acquiring the QueryObject (we can open up a new stackoverflow question on that if necessary).

It's a multi-stage thing. We'll assume you started with 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.
  }
}

(I'm about 98% sure the code will work as is... I typed it on the fly and can't guarantee it'll just copy and paste.)

Edit --

I realized I only answered half of your question. How to get box dimensions. You still want to know where the box is so you can compute box vertices. I've modified the example code to do so.

Upvotes: 2

Related Questions