Compute bounding based on GeometryId #23133

xuchenhan-tri · 2025-06-26T23:04:03Z

This change is

xuchenhan-tri

-(status: do not review)

+@SeanCurtis-TRI could you give me a design review for this?

This PR implements what we discussed last week about supporting querying bounding boxes in the world frame for a registered geometry.

This PR adds
QueryObject::ComputeAabbInWorld (only works for deformable geometries)
QueryObject::ComputeObbInWorld (only works for rigid geometries)
SceneGraphInspector::GetObbInGeometryFrame and a free function CalcObb that works for all shapes for which the concept of bounding boxes make sense.

For Mesh and Convex shape, I followed the pattern of computing convex hulls (performing the computation at the first invocation and retrieving the pre-computed values at subsequent invocations). To support that, I also had to do some dependency surgeries on Obb.

Some of the changes are from #23120. I'll rebase when that's merged.

This PR should close #15121 and #20565.

Reviewable status: LGTM missing from assignee SeanCurtis-TRI(platform), needs at least two assigned reviewers, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @xuchenhan-tri)

jwnimmer-tri

Reviewed 9 of 34 files at r1.
Reviewable status: 9 unresolved discussions, LGTM missing from assignee SeanCurtis-TRI(platform), needs at least two assigned reviewers, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @xuchenhan-tri)

geometry/shape_specification.cc line 71 at r1 (raw file):

Note: the correctness of this function is tested in shape_specification_thread_test.cc.

That doesn't seem true, given that the test is not changed in this commit.

geometry/shape_specification.cc line 75 at r1 (raw file):

                           const MeshSource& mesh_source,
                           const Vector3<double>& scale) {
  std::shared_ptr<Obb> check = std::atomic_load(obb_ptr);

nit See above in the similar convex hull function. We need to replicate the (recently added) #pragma GCC diagnostic push around the atomics in this functions.

geometry/shape_specification.cc line 466 at r1 (raw file):

        // For a box, the OBB is aligned with the geometry frame and centered at
        // origin.
        return Obb(math::RigidTransform<double>::Identity(), box.size() / 2);

nit We have a using math::RigidTransform already in this file. (Though it should probably move to the top of the file so we can see it more easily.)

Suggestion:

RigidTransform<double>

geometry/shape_specification.cc line 473 at r1 (raw file):

        const double radius = capsule.radius();
        const double half_length = capsule.length() / 2.0 + radius;
        const Vector3<double> half_size(radius, radius, half_length);

BTW Calling this "half_length" is slightly confusing, depending on how the reader interprets "length". (Or if you want to keep the math like this, maybe call it half_z or something.)

Suggestion:

        const double half_length = capsule.length() / 2.0;
        const Vector3<double> half_size(radius, radius, half_length + radius);

geometry/query_object.h line 199 at r1 (raw file):

   `geometry_id` in the world frame.
   @throws std::exception if the geometry `geometry_id` is not valid.
   @throws std::exception if the geometry is not deformable.  */

BTW Is this limitation ("not deformable" => throws) something we want?

In this contract, we are promising that we'll always throw, which means implementing support for rigid aabb in the future would be a deprecation-requiring breaking change.

Instead, phrasing the documentation here as a current limitation of the implementation would open the door to adding it in later, without a breaking change.

(Alternatively, both of these methods could return optional<> and throw only if the geometry_id is invalid, instead using a nullopt-return to indicate lack of support?)

geometry/shape_specification.h line 362 at r1 (raw file):

   shape's canonical frame.

   Note: the convex hull is computed on demand on the first invocation. All

typo

Suggestion:

OBB

geometry/shape_specification.h line 629 at r1 (raw file):

   canonical frame.

   Note: the convex hull is computed on demand on the first invocation. All

typo

Suggestion:

OBB

geometry/scene_graph_inspector.h line 434 at r1 (raw file):

  /** Returns the oriented bounding box associated with the given `geometry_id`
   in the geometry's frame. The OBB is defined in the geometry's canonical frame

BTW GetReferenceMesh docs mention G frame for precision when describing the geometry frame. Possibly we should also do that here?

geometry/proximity/BUILD.bazel line 131 at r1 (raw file):

)

drake_cc_library(

Working

I am not yet convinced about this cycle-split. I will dig further.

xuchenhan-tri

Reviewable status: 1 unresolved discussion, LGTM missing from assignee SeanCurtis-TRI(platform), needs at least two assigned reviewers, labeled "do not merge", commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @xuchenhan-tri)

geometry/query_object.h line 199 at r1 (raw file):