Compute bounding based on GeometryId

bindings/pydrake/geometry/geometry_py_scene_graph.cc

@@ -184,6 +184,8 @@ void DefineSceneGraphInspector(py::module m, T) {
             cls_doc.GetAllDeformableGeometryIds.doc)
         .def("GetConvexHull", &Class::GetConvexHull, py_rvp::reference_internal,
             py::arg("geometry_id"), cls_doc.GetConvexHull.doc)
+        .def("GetObbInGeometryFrame", &Class::GetObbInGeometryFrame,
+            py::arg("geometry_id"), cls_doc.GetObbInGeometryFrame.doc)
         .def("CollisionFiltered", &Class::CollisionFiltered,
             py::arg("geometry_id1"), py::arg("geometry_id2"),
             cls_doc.CollisionFiltered.doc)
@@ -566,16 +568,20 @@ void DefineQueryObject(py::module m, T) {
                 &Class::GetPoseInWorld),
             py::arg("frame_id"), py_rvp::reference_internal,
             cls_doc.GetPoseInWorld.doc_1args_frame_id)
-        .def("GetConfigurationsInWorld", &Class::GetConfigurationsInWorld,
-            py::arg("deformable_geometry_id"), py_rvp::copy,
-            cls_doc.GetConfigurationsInWorld.doc)
         .def("GetPoseInParent", &Class::GetPoseInParent, py::arg("frame_id"),
             py_rvp::reference_internal, cls_doc.GetPoseInParent.doc)
         .def("GetPoseInWorld",
             overload_cast_explicit<const math::RigidTransform<T>&, GeometryId>(
                 &Class::GetPoseInWorld),
             py::arg("geometry_id"), py_rvp::reference_internal,
             cls_doc.GetPoseInWorld.doc_1args_geometry_id)
+        .def("GetConfigurationsInWorld", &Class::GetConfigurationsInWorld,
+            py::arg("deformable_geometry_id"), py_rvp::copy,
+            cls_doc.GetConfigurationsInWorld.doc)
+        .def("ComputeAabbInWorld", &Class::ComputeAabbInWorld,
+            py::arg("geometry_id"), cls_doc.ComputeAabbInWorld.doc)
+        .def("ComputeObbInWorld", &Class::ComputeObbInWorld,
+            py::arg("geometry_id"), cls_doc.ComputeObbInWorld.doc)
         .def("ComputeSignedDistancePairwiseClosestPoints",
             &QueryObject<T>::ComputeSignedDistancePairwiseClosestPoints,
             py::arg("max_distance") = std::numeric_limits<double>::infinity(),

bindings/pydrake/geometry/test/scene_graph_test.py

@@ -279,6 +279,17 @@ def test_scene_graph_api(self, T):
         self.assertTrue(inspector.IsDeformableGeometry(geometry_id=sphere_4))
         self.assertEqual(inspector.GetAllDeformableGeometryIds(), [sphere_4])
         self.assertIsNone(inspector.GetConvexHull(geometry_id=sphere_3))
+        # Test GetObbInGeometryFrame for a geometry that supports it.
+        self.assertIsInstance(
+            inspector.GetObbInGeometryFrame(geometry_id=sphere_3), mut.Obb)
+        # Test GetObbInGeometryFrame for a geometry that does not support it.
+        half_space = scene_graph.RegisterAnchoredGeometry(
+            source_id=global_source,
+            geometry=mut.GeometryInstance(X_PG=RigidTransform_[float](),
+                                          shape=mut.HalfSpace(),
+                                          name="half_space"))
+        self.assertIsNone(
+            inspector.GetObbInGeometryFrame(geometry_id=half_space))
         self.assertIsInstance(inspector.geometry_version(),
                               mut.GeometryVersion)
 
@@ -678,6 +689,13 @@ def test_query_object(self, T):
         self.assertIsInstance(
             query_object.GetPoseInWorld(geometry_id=geometry_id),
             RigidTransform_[T])
+        self.assertIsInstance(
+            query_object.ComputeAabbInWorld(
+                geometry_id=deformable_geometry_id),
+            mut.Aabb)
+        self.assertIsInstance(
+            query_object.ComputeObbInWorld(geometry_id=geometry_id),
+            mut.Obb)
 
         # Proximity queries -- all of these will produce empty results.
         results = query_object.ComputeSignedDistancePairwiseClosestPoints()

geometry/BUILD.bazel

@@ -456,13 +456,15 @@ drake_cc_library(
     deps = [
         ":mesh_source",
         "//common:essential",
+        "//geometry/proximity:obb",
         "//geometry/proximity:polygon_surface_mesh",
         "//math:geometric_transform",
     ],
     implementation_deps = [
         "//common:nice_type_name",
         "//common:overloaded",
         "//geometry/proximity:make_convex_hull_mesh_impl",
+        "//geometry/proximity:make_obb_from_mesh",
         "//geometry/proximity:meshing_utilities",
         "//geometry/proximity:obj_to_surface_mesh",
         "//geometry/proximity:polygon_to_triangle_mesh",

geometry/geometry_state.cc

@@ -7,6 +7,7 @@
 #include <string>
 #include <utility>
 
+#include "proximity/volume_mesh.h"
 #include <fmt/format.h>
 #include <fmt/ranges.h>
 
@@ -18,6 +19,7 @@
 #include "drake/geometry/geometry_instance.h"
 #include "drake/geometry/geometry_roles.h"
 #include "drake/geometry/proximity/make_convex_hull_mesh.h"
+#include "drake/geometry/proximity/obb.h"
 #include "drake/geometry/proximity/volume_to_surface_mesh.h"
 #include "drake/geometry/proximity_engine.h"
 #include "drake/geometry/proximity_properties.h"
@@ -808,6 +810,13 @@ const PolygonSurfaceMesh<double>* GeometryState<T>::GetConvexHull(
   return extractor.GetConvexHull();
 }
 
+template <typename T>
+std::optional<Obb> GeometryState<T>::GetObbInGeometryFrame(
+    GeometryId id) const {
+  const InternalGeometry& geometry = GetValueOrThrow(id, geometries_);
+  return CalcObb(geometry.shape());
+}
+
 template <typename T>
 bool GeometryState<T>::CollisionFiltered(GeometryId id1, GeometryId id2) const {
   std::string base_message =
@@ -867,6 +876,64 @@ const math::RigidTransform<T>& GeometryState<T>::get_pose_in_world(
   return kinematics_data_.X_WGs.at(geometry_id);
 }
 
+template <typename T>
+std::optional<Aabb> GeometryState<T>::ComputeAabbInWorld(
+    GeometryId geometry_id) const {
+  FindOrThrow(geometry_id, geometries_, [geometry_id]() {
+    return "No AABB available for invalid geometry id: " +
+           to_string(geometry_id);
+  });
+  const auto& geometry = GetValueOrThrow(geometry_id, geometries_);
+  // For non-deformable geometries, we don't support computing the AABB (yet).
+  if (!geometry.is_deformable()) {
+    return std::nullopt;
+  }
+  // For deformable geometries with proximity role, the proximity engine
+  // already keeps track of the AABB.
+  if (geometry.has_proximity_role()) {
+    return geometry_engine_->GetDeformableAabbInWorld(geometry_id);
+  }
+  // For deformable geometries without proximity role, we need to manually
+  // deform the geometry and compute the AABB of the deformed mesh.
+  const VolumeMesh<double>& reference_mesh = *geometry.reference_mesh();
+  std::vector<Vector3<double>> deformed_vertices(reference_mesh.num_vertices());
+  const VectorX<double>& q_WG =
+      convert_to_double(kinematics_data_.q_WGs.at(geometry_id));
+  for (int i = 0; i < reference_mesh.num_vertices(); ++i) {
+    deformed_vertices[i] = q_WG.template segment<3>(3 * i);
+  }
+  auto tets = reference_mesh.tetrahedra();
+  const VolumeMesh<double> deformed_mesh(std::move(tets),
+                                         std::move(deformed_vertices));
+  std::set<int> vertex_indices;
+  for (int i = 0; i < deformed_mesh.num_vertices(); ++i) {
+    vertex_indices.insert(i);
+  }
+  return AabbMaker<VolumeMesh<double>>(deformed_mesh, vertex_indices).Compute();
+}
+
+template <typename T>
+std::optional<Obb> GeometryState<T>::ComputeObbInWorld(
+    GeometryId geometry_id) const {
+  FindOrThrow(geometry_id, geometries_, [geometry_id]() {
+    return "No OBB available for invalid geometry id: " +
+           to_string(geometry_id);
+  });
+  const auto& geometry = GetValueOrThrow(geometry_id, geometries_);
+  // For deformable geometries, we don't support computing the OBB (yet).
+  if (geometry.is_deformable()) {
+    return std::nullopt;
+  }
+  const std::optional<Obb> obb_G = GetObbInGeometryFrame(geometry_id);
+  if (!obb_G.has_value()) {
+    return std::nullopt;
+  }
+  const math::RigidTransform<double>& X_WG =
+      convert_to_double(get_pose_in_world(geometry_id));
+  const math::RigidTransform<double>& X_GB = obb_G->pose();
+  return Obb(X_WG * X_GB, obb_G->half_width());
+}
+
 template <typename T>
 const math::RigidTransform<T>& GeometryState<T>::get_pose_in_parent(
     FrameId frame_id) const {

geometry/geometry_state.h

@@ -396,6 +396,9 @@ class GeometryState {
   /** Implementation of SceneGraphInspector::GetConvexHull(). */
   const PolygonSurfaceMesh<double>* GetConvexHull(GeometryId id) const;
 
+  /** Implementation of SceneGraphInspector::GetObbInGeometryFrame(). */
+  std::optional<Obb> GetObbInGeometryFrame(GeometryId id) const;
+
   /** Implementation of SceneGraphInspector::CollisionFiltered().  */
   bool CollisionFiltered(GeometryId id1, GeometryId id2) const;
 
@@ -415,6 +418,12 @@ class GeometryState {
   const math::RigidTransform<T>& get_pose_in_world(
       GeometryId geometry_id) const;
 
+  /** Implementation of QueryObject::ComputeAabbInWorld(GeometryId).  */
+  std::optional<Aabb> ComputeAabbInWorld(GeometryId geometry_id) const;
+
+  /** Implementation of QueryObject::ComputeObbInWorld(GeometryId).  */
+  std::optional<Obb> ComputeObbInWorld(GeometryId geometry_id) const;
+
   /** Implementation of QueryObject::GetPoseInParent().  */
   const math::RigidTransform<T>& get_pose_in_parent(FrameId frame_id) const;
 

geometry/proximity/BUILD.bazel

@@ -44,6 +44,7 @@ drake_cc_package_library(
         ":make_ellipsoid_mesh",
         ":make_mesh_field",
         ":make_mesh_from_vtk",
+        ":make_obb_from_mesh",
         ":make_sphere_field",
         ":make_sphere_mesh",
         ":mesh_field",
@@ -53,6 +54,8 @@ drake_cc_package_library(
         ":mesh_to_vtk",
         ":mesh_traits",
         ":meshing_utilities",
+        ":obb",
+        ":obb_has_overlap",
         ":obj_to_surface_mesh",
         ":plane",
         ":polygon_surface_mesh",
@@ -93,14 +96,14 @@ drake_cc_library(
     name = "bv",
     srcs = [
         "aabb.cc",
-        "obb.cc",
     ],
     hdrs = [
         "aabb.h",
-        "obb.h",
     ],
     deps = [
         ":boxes_overlap",
+        ":obb",
+        ":obb_has_overlap",
         ":posed_half_space",
         ":triangle_surface_mesh",
         ":volume_mesh",
@@ -111,6 +114,32 @@ drake_cc_library(
     ],
 )
 
+drake_cc_library(
+    name = "obb",
+    srcs = ["obb.cc"],
+    hdrs = ["obb.h"],
+    deps = [
+        ":polygon_surface_mesh",
+        ":triangle_surface_mesh",
+        ":volume_mesh",
+        "//common:essential",
+        "//geometry:utilities",
+        "//math:geometric_transform",
+    ],
+)
+
+drake_cc_library(
+    name = "obb_has_overlap",
+    srcs = ["obb_has_overlap.cc"],
+    hdrs = ["aabb.h"],
+    deps = [
+        ":boxes_overlap",
+        ":obb",
+        ":plane",
+        "//geometry:shape_specification",
+    ],
+)
+
 drake_cc_library(
     name = "bvh",
     srcs = ["bvh.cc"],
@@ -558,6 +587,25 @@ drake_cc_library(
     ],
 )
 
+drake_cc_library(
+    name = "make_obb_from_mesh",
+    srcs = ["make_obb_from_mesh.cc"],
+    hdrs = ["make_obb_from_mesh.h"],
+    deps = [
+        ":obb",
+        ":obj_to_surface_mesh",
+        ":polygon_surface_mesh",
+        ":triangle_surface_mesh",
+        ":volume_mesh",
+        ":vtk_to_volume_mesh",
+        "//common:essential",
+        "//geometry:mesh_source",
+    ],
+    implementation_deps = [
+        ":make_convex_hull_mesh_impl",
+    ],
+)
+
 drake_cc_library(
     name = "make_convex_hull_mesh",
     srcs = ["make_convex_hull_mesh.cc"],
@@ -1444,6 +1492,22 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "make_obb_from_mesh_test",
+    data = [
+        "//geometry:test_gltf_files",
+        "//geometry:test_obj_files",
+        "//geometry:test_vtk_files",
+    ],
+    deps = [
+        ":make_obb_from_mesh",
+        "//common:find_resource",
+        "//common/test_utilities:eigen_matrix_compare",
+        "//common/test_utilities:expect_throws_message",
+        "//geometry:mesh_source",
+    ],
+)
+
 drake_cc_googletest(
     name = "make_cylinder_field_test",
     deps = [
@@ -1627,9 +1691,13 @@ drake_cc_googletest(
 
 drake_cc_googletest(
     name = "obb_test",
+    data = [
+        "//geometry:test_obj_files",
+    ],
     deps = [
         ":bv",
         ":make_box_mesh",
+        ":make_convex_hull_mesh",
         ":make_ellipsoid_mesh",
         ":make_sphere_mesh",
         ":triangle_surface_mesh",

geometry/proximity/deformable_contact_geometries.h

@@ -106,6 +106,12 @@ class DeformableGeometry {
    instead of kept around. */
   const VolumeMeshFieldLinear<double, double>& CalcSignedDistanceField() const;
 
+  /* Returns the axis-aligned bounding box of the geometry in the world frame.
+   */
+  const Aabb& aabb_in_world() const {
+    return deformable_volume_->bvh().root_node().bv();
+  }
+
  private:
   std::unique_ptr<DeformableVolumeMeshWithBvh<double>> deformable_volume_;
   std::unique_ptr<DeformableSurfaceMeshWithBvh<double>> deformable_surface_;

geometry/proximity/deformable_contact_internal.cc

@@ -163,6 +163,14 @@ DeformableContact<double> Geometries::ComputeDeformableContact(
   return result;
 }
 
+const Aabb& Geometries::GetDeformableAabbInWorld(GeometryId geometry_id) const {
+  if (is_deformable(geometry_id)) {
+    return deformable_geometries_.at(geometry_id).aabb_in_world();
+  }
+  throw std::runtime_error(
+      "GetDeformableAabbInWorld: Geometry is not deformable.");
+}
+
 void Geometries::ImplementGeometry(const Box& box, void* user_data) {
   AddRigidGeometry(box, *static_cast<ReifyData*>(user_data));
 }

geometry/proximity/deformable_contact_internal.h

@@ -135,6 +135,10 @@ class Geometries final : public ShapeReifier {
   DeformableContact<double> ComputeDeformableContact(
       const CollisionFilter& collision_filter) const;
 
+  /* Returns the axis-aligned bounding box of the geometry with the given id.
+   @pre The geometry with the given id must have a deformable representation. */
+  const Aabb& GetDeformableAabbInWorld(GeometryId geometry_id) const;
+
  private:
   friend class GeometriesTester;