Conception/drake-master/geometry/query_object.h

#pragma once

#include <limits>
#include <memory>
#include <string>
#include <vector>

#include "drake/common/drake_deprecated.h"
#include "drake/geometry/query_results/contact_surface.h"
#include "drake/geometry/query_results/deformable_contact.h"
#include "drake/geometry/query_results/penetration_as_point_pair.h"
#include "drake/geometry/query_results/signed_distance_pair.h"
#include "drake/geometry/query_results/signed_distance_to_point.h"
#include "drake/geometry/render/render_camera.h"
#include "drake/geometry/render/render_engine.h"
#include "drake/geometry/scene_graph_inspector.h"
#include "drake/math/rigid_transform.h"
#include "drake/systems/framework/context.h"
#include "drake/systems/sensors/image.h"

namespace drake {
namespace geometry {

template <typename T>
class SceneGraph;

/** The %QueryObject serves as a mechanism to perform geometry queries on the
 world's geometry. The SceneGraph has an abstract-valued port that contains
 a  %QueryObject (i.e., a %QueryObject-valued output port).

 To perform geometry queries on SceneGraph:
   - a LeafSystem must have a %QueryObject-valued input port and connect it to
     the corresponding query output port on SceneGraph,
   - the querying LeafSystem can evaluate the input port, retrieving a `const
     QueryObject&` in return, and, finally,
   - invoke the appropriate method on the %QueryObject.

 The const reference returned by the input port is considered "live" - it is
 linked to the context, system, and cache (making full use of all of those
 mechanisms). This const reference should _never_ be persisted; doing so can
 lead to erroneous query results. It is simpler and more advisable to acquire it
 for evaluation in a limited scope (e.g., CalcTimeDerivatives()) and then
 discard it. If a %QueryObject is needed for many separate functions in a
 LeafSystem, each should re-evaluate the input port. The underlying caching
 mechanism should make the cost of this negligible.

 The %QueryObject _can_ be copied. The copied instance is no longer "live"; it
 is now "baked". Essentially, it freezes the state of the live scene graph in
 its current configuration and disconnects it from the system and context. This
 means, even if the original context changes values, the copied/baked instance
 will always reproduce the same query results. This baking process is not cheap
 and should not be done without consideration.

 @anchor query_object_precision_methodology
 <h2>Queries and scalar type</h2>

 A %QueryObject _cannot_ be converted to a different scalar type. A %QueryObject
 of scalar type T can only be acquired from the output port of a SceneGraph
 of type T evaluated on a corresponding Context, also of type T.

 %QueryObject's support for arbitrary scalar type is incomplete. Not all queries
 support all scalar types to the same degree. Furthermore, the queries are
 typically served by *families* of algorithms. The evaluation of a query between
 a particular pair of geometries will depend on the query, the pair of geometry
 types involved, and the scalar type. From query to query, the treatment of
 a geometry (or geometry pair) for a given scalar type can vary in many ways,
 including but not limited to: ignoring the geometry, throwing an exception,
 results with limited precision, or full, accurate support. The queries below
 provide tables to help inform your expectations when evaluating queries. The
 tables all use a common methodology and admit a common interpretation.

 For each (query, geometry-pair, scalar) combination, we create a set of
 geometric configurations with known answers. We evaluate the precision of the
 query result (if supported at all) over the entire set and report the *worst*
 observed error. This is a purely empirical approach and doesn't fully
 characterize the families of underlying algorithms, and the reported error
 may be misleading in that we've missed far worse latent error or that the
 error reported doesn't well represent the average case.

 The families of algorithms also differ in how their inherent errors scale with
 the scale of the problem (e.g., size of geometries, magnitude of
 distance/depth, etc.) Attempting to fully characterize that aspect is both
 arduous and problematic, so, we've chosen a more *representative* approach.

 Because Drake is primarily intended for robot simulation, we've created
 geometric configurations on the scale of common robot manipulators (on the
 order of 20cm). We position them with a known penetration depth (or separating
 distance) of 2 mm. The error reported is the deviation from that expected
 result.

 When interpreting the tables, keep the following in mind:
   - The table illustrates trends in *broad* strokes, only. It does not
     represent an exhaustive analysis.
   - If your problem involves larger geometries, greater penetration depths, or
     larger separating distances, the error will vary. Do not assume that
     observed error in this context is necessarily relative -- there truly is
     that much variability in the families of algorithms.
   - These values are an attempt to capture the *worst* case outcome. The
     error shown is a single-significant-digit approximation of that observed
     error.
   - These values may not actually represent the true worst case; discovering
     the true worst case is generally challenging. These represent our best
     effort to date. If you find outcomes that are worse those reported here,
     please <a href="https://github.com/RobotLocomotion/drake/issues/new">
     submit a bug</a>.
   - These tables represent Drake's transient state. The eventual goal is to
     report no more than 1e-14 error across all supportable geometry pairs
     and scalars. At that point, the table will simply disappear.

 @tparam_default_scalar
*/
template <typename T>
class QueryObject {
 public:
  /** Constructs a default QueryObject (all pointers are null). */
  QueryObject() = default;

  /** @name Implements CopyConstructible, CopyAssignable, \
                       MoveConstructible, MoveAssignable

   Calling the copy constructor or assignment will turn a _live_ %QueryObject
   into a _baked_ %QueryObject (an expensive operation). Copying baked
   QueryObjects is cheap.  */
  //@{

  QueryObject(const QueryObject& other);
  QueryObject& operator=(const QueryObject&);
  QueryObject(QueryObject&&) = default;
  QueryObject& operator=(QueryObject&&) = default;

  //@}

  // Note to developers on adding queries:
  //  All queries should call ThrowIfNotCallable() before taking any action.
  //  Furthermore, an invocation of that query method should be included in
  //  query_object_test.cc in the DefaultQueryThrows test to confirm that the
  //  query *is* calling ThrowIfNotCallable().

  /** Provides an inspector for the topological structure of the underlying
   scene graph data (see SceneGraphInspector for details).  */
  const SceneGraphInspector<T>& inspector() const {
    return inspector_;
  }

  /** @name                Configuration-dependent Introspection

   These methods provide access to introspect geometry and frame quantities that
   directly depend on the poses of the frames or configurations of deformable
   geometries. For geometry and frame quantities that do not depend on the poses
   of frames, such as  X_FG, use inspector() to access the SceneGraphInspector.
  */
  //@{

  /** Reports the position of the frame indicated by `frame_id` relative to the
   world frame.
   @throws std::exception if the frame `frame_id` is not valid.  */
  const math::RigidTransform<T>& GetPoseInWorld(FrameId frame_id) const;

  /** Reports the position of the frame indicated by `frame_id` relative to its
   parent frame. If the frame was registered with the world frame as its parent
   frame, this value will be identical to that returned by GetPoseInWorld().
   <!-- 2023-04-01 Remove this note when we're done deprecating
    SGI::GetPoseInParent(). -->
   @note This is analogous to but distinct from
   SceneGraphInspector::GetPoseInParent(). In this case, the pose will *always*
   be relative to another frame.
   @throws std::exception if the frame `frame_id` is not valid.  */
  const math::RigidTransform<T>& GetPoseInParent(FrameId frame_id) const;

  /** Reports the position of the frame of the rigid geometry indicated by
   `geometry_id` relative to the world frame (X_WG).
   @note This query is meaningless for deformable geometries. Their current
   state cannot be represented by a single rigid transformation. Instead, one
   should use GetConfigurationsInWorld() to get the current vertex positions of
   the deformable geometry in the world frame. On the other hand, it _is_
   meaningful to query the *fixed* pose of the *reference* geometry in its
   parent frame (see SceneGraphInspector::GetPoseInFrame()).
   @throws std::exception if the geometry `geometry_id` is not valid or if it
   is deformable.  */
  const math::RigidTransform<T>& GetPoseInWorld(GeometryId geometry_id) const;

  /** Reports the configuration of the deformable geometry indicated by
   `geometry_id` relative to the world frame.
   @sa GetPoseInWorld().
   @throws std::exception if the geometry `geometry_id` is not valid or is not
   a deformable geometry.  */
  const VectorX<T>& GetConfigurationsInWorld(
      GeometryId deformable_geometry_id) const;

  //@}

  /**
   @anchor collision_queries
   @name                Collision Queries

   These queries detect _collisions_ between geometry. Two geometries collide
   if they overlap each other and are not explicitly excluded through
   @ref scene_graph_collision_filter_manager "collision filtering".
   These algorithms find those colliding cases, characterize them, and report
   the essential characteristics of that collision.

   For two colliding geometries g_A and g_B, it is guaranteed that they will
   map to `id_A` and `id_B` in a fixed, repeatable manner, where `id_A` and
   `id_B` are GeometryId's of geometries g_A and g_B respectively.

   These methods are affected by collision filtering; element pairs that
   have been filtered will not produce contacts, even if their collision
   geometry is penetrating.     */
  //@{

  /** Computes the penetrations across all pairs of geometries in the world
   with the penetrations characterized by pairs of points (see
   PenetrationAsPointPair), providing some measure of the penetration "depth" of
   the two objects, but _not_ the overlapping volume.

   Only reports results for _penetrating_ geometries; if two geometries are
   separated, there will be no result for that pair. Geometries whose surfaces
   are just touching (osculating) are not considered in penetration. Surfaces
   whose penetration is within an epsilon of osculation, are likewise not
   considered penetrating. Pairs of _anchored_ geometry are also not reported.
   This method is affected by collision filtering.

   For two penetrating geometries g_A and g_B, it is guaranteed that they will
   map to `id_A` and `id_B` in a fixed, repeatable manner.

   <h3>Characterizing the returned values</h3>

   As discussed in the
   @ref query_object_precision_methodology "class's documentation", these tables
   document the support given by this query for pairs of geometry types and
   scalar. See the description in the link for details on how to interpret the
   tables' results. The query is symmetric with respect to shape *ordering*, the
   pair (ShapeA, ShapeB) will be the same as (ShapeB, ShapeA), so we only fill
   in half of each table.

   |           |   %Box  | %Capsule | %Convex | %Cylinder | %Ellipsoid | %HalfSpace |  %Mesh  | %Sphere |
   | --------: | :-----: | :------: | :-----: | :-------: | :--------: | :--------: | :-----: | :-----: |
   | Box       |  2e-15  |  ░░░░░░  |  ░░░░░  |  ░░░░░░░  |   ░░░░░░   |   ░░░░░░   |  ░░░░░  |  ░░░░░  |
   | Capsule   |  3e-5ᶜ  |   2e-5ᶜ  |  ░░░░░  |  ░░░░░░░  |   ░░░░░░   |   ░░░░░░   |  ░░░░░  |  ░░░░░  |
   | Convex    |  2e-15ᶜ |   3e-5ᶜ  | 2e-15ᶜ  |  ░░░░░░░  |   ░░░░░░   |   ░░░░░░   |  ░░░░░  |  ░░░░░  |
   | Cylinder  |  1e-3ᶜ  |   4e-5ᶜ  |  1e-3ᶜ  |   2e-3ᶜ   |   ░░░░░░   |   ░░░░░░   |  ░░░░░  |  ░░░░░  |
   | Ellipsoid |  4e-4ᶜ  |   2e-4ᶜ  |  4e-4ᶜ  |   2e-3ᶜ   |    5e-4ᶜ   |   ░░░░░░   |  ░░░░░  |  ░░░░░  |
   | HalfSpace |  6e-15  |   4e-15  | 3e-15ᶜ  |   4e-15   |   3e-15    |   throwsᵃ  |  ░░░░░  |  ░░░░░  |
   | Mesh      |    ᵇ    |    ᵇ     |    ᵇ    |     ᵇ     |      ᵇ     |     ᵇ      |    ᵇ    |  ░░░░░  |
   | Sphere    |  3e-15  |   5e-15  |  3e-5ᶜ  |   5e-15   |    2e-4ᶜ   |   3e-15    |    ᵇ    |  5e-15  |
   __*Table 1*__: Worst observed error (in m) for 2mm penetration between
   geometries approximately 20cm in size for `T` = `double`.

   |           |   %Box  | %Capsule | %Convex | %Cylinder | %Ellipsoid | %HalfSpace |  %Mesh  | %Sphere |
   | --------: | :-----: | :------: | :-----: | :-------: | :--------: | :--------: | :-----: | :-----: |
   | Box       | throwsᵈ |  ░░░░░░  |  ░░░░░  |  ░░░░░░░  |   ░░░░░░   |   ░░░░░░   |  ░░░░░  |  ░░░░░  |
   | Capsule   | throwsᵈ | throwsᵈ  |  ░░░░░  |  ░░░░░░░  |   ░░░░░░   |   ░░░░░░   |  ░░░░░  |  ░░░░░  |
   | Convex    | throwsᵈ | throwsᵈ  | throwsᵈ |  ░░░░░░░  |   ░░░░░░   |   ░░░░░░   |  ░░░░░  |  ░░░░░  |
   | Cylinder  | throwsᵈ | throwsᵈ  | throwsᵈ |  throwsᵈ  |   ░░░░░░   |   ░░░░░░   |  ░░░░░  |  ░░░░░  |
   | Ellipsoid | throwsᵈ | throwsᵈ  | throwsᵈ |  throwsᵈ  |   throwsᵈ  |   ░░░░░░   |  ░░░░░  |  ░░░░░  |
   | HalfSpace | throwsᵈ | throwsᵈ  | throwsᵈ |  throwsᵈ  |   throwsᵈ  |   throwsᵃ  |  ░░░░░  |  ░░░░░  |
   | Mesh      |    ᵇ    |    ᵇ     |    ᵇ    |     ᵇ     |      ᵇ     |     ᵇ      |    ᵇ    |  ░░░░░  |
   | Sphere    |  2e-15  |  3e-15   | throwsᵈ |   2e-15   |   throwsᵈ  |   2e-15    |    ᵇ    |  5e-15  |
   __*Table 2*__: Worst observed error (in m) for 2mm penetration between
   geometries approximately 20cm in size for `T` = @ref drake::AutoDiffXd
   "AutoDiffXd".

   |           |   %Box  | %Capsule | %Convex | %Cylinder | %Ellipsoid | %HalfSpace |  %Mesh  | %Sphere |
   | --------: | :-----: | :------: | :-----: | :-------: | :--------: | :--------: | :-----: | :-----: |
   | Box       | throwsᵉ |  ░░░░░░  |  ░░░░░  |  ░░░░░░░  |   ░░░░░░   |   ░░░░░░   |  ░░░░░  |  ░░░░░  |
   | Capsule   | throwsᵉ | throwsᵉ  |  ░░░░░  |  ░░░░░░░  |   ░░░░░░   |   ░░░░░░   |  ░░░░░  |  ░░░░░  |
   | Convex    | throwsᵉ | throwsᵉ  | throwsᵉ |  ░░░░░░░  |   ░░░░░░   |   ░░░░░░   |  ░░░░░  |  ░░░░░  |
   | Cylinder  | throwsᵉ | throwsᵉ  | throwsᵉ |  throwsᵉ  |   ░░░░░░   |   ░░░░░░   |  ░░░░░  |  ░░░░░  |
   | Ellipsoid | throwsᵉ | throwsᵉ  | throwsᵉ |  throwsᵉ  |   throwsᵉ  |   ░░░░░░   |  ░░░░░  |  ░░░░░  |
   | HalfSpace | throwsᵉ | throwsᵉ  | throwsᵉ |  throwsᵉ  |   throwsᵉ  |   throwsᵉ  |  ░░░░░  |  ░░░░░  |
   | Mesh      |    ᵇ    |    ᵇ     |    ᵇ    |     ᵇ     |      ᵇ     |     ᵇ      |    ᵇ    |  ░░░░░  |
   | Sphere    | throwsᵉ | throwsᵉ  | throwsᵉ |  throwsᵉ  |   throwsᵉ  |    throwsᵉ  |    ᵇ    | throwsᵉ |
   __*Table 3*__: Support for `T` = @ref drake::symbolic::Expression.

   - ᵃ Penetration depth between two HalfSpace instances has no meaning; either
       they don't intersect, or they have infinite penetration.
   - ᵇ Meshes are represented by the *convex* hull of the mesh, therefore the
       results for Mesh are assumed to be the same as for Convex.
   - ᶜ These results are computed using an iterative algorithm. For particular
       configurations, the solution may be correct to machine precision. The
       values reported here are confirmed, observed worst case answers.
   - ᵈ These results are simply not supported for
       `T` = @ref drake::AutoDiffXd "AutoDiffXd" at this time.
   - ᵉ These results are simply not supported for
       `T` = @ref drake::symbolic::Expression at this time.

   <!-- Note to developers: the tests that support the assertions here are
   located in penetration_as_point_pair_characterize_test.cc. The values in this
   table should be reflected in the expected values there.  -->

   @returns A vector populated with all detected penetrations characterized as
            point pairs. The ordering of the results is guaranteed to be
            consistent -- for fixed geometry poses, the results will remain
            the same.
   @warning For Mesh shapes, their convex hulls are used in this query. It is
            *not* computationally efficient or particularly accurate.
   @throws std::exception if a Shape-Shape pair is in collision and indicated as
           `throws` in the support table above.  */
  std::vector<PenetrationAsPointPair<T>> ComputePointPairPenetration() const;

  /** Reports pairwise intersections and characterizes each non-empty
   intersection as a ContactSurface for hydroelastic contact model. The
   computation is subject to collision filtering.

   For two intersecting geometries g_A and g_B, it is guaranteed that they will
   map to `id_A` and `id_B` in a fixed, repeatable manner, where `id_A` and
   `id_B` are GeometryId's of geometries g_A and g_B respectively.

   In the current incarnation, this function represents an incomplete
   implementation. That has several implications, as described below:

     - This table shows which shapes can be declared for use in hydroelastic
       contact, and what compliance can be assigned.

       |   Shape   | Compliant | Rigid |
       | :-------: | :-------: | :---: |
       | Sphere    |    yes    |  yes  |
       | Cylinder  |    yes    |  yes  |
       | Box       |    yes    |  yes  |
       | Capsule   |    yes    |  yes  |
       | Ellipsoid |    yes    |  yes  |
       | HalfSpace |    yes    |  yes  |
       | Mesh      |    no     |  yes  |
       | Convex    |    yes    |  yes  |

     - We do not support contact between two rigid geometries. One geometry
       *must* be compliant, and the other could be rigid or compliant. If
       two rigid geometries collide, an exception will be thrown. More
       particularly, if such a geometry pair *cannot be culled* an exception
       will be thrown. No exception is thrown if the pair has been filtered.
     - If you need all combinations of rigid-rigid contact, rigid-compliant
       contact, and compliant-compliant contact, you might consider
       ComputeContactSurfacesWithFallback().
     - The hydroelastic modulus (N/m^2) of each compliant geometry is set in
       ProximityProperties by AddCompliantHydroelasticProperties().
     - The tessellation of the corresponding meshes is controlled by the
       resolution hint (where appropriate), as defined by
       AddCompliantHydroelasticProperties() and
       AddRigidHydroelasticProperties().

   <h3>Scalar support</h3>

   This method provides support for both double and AutoDiffXd, but not
   Expression. Like with the other proximity queries, derivatives can only be
   introduced via geometry *poses*. We cannot differentiate w.r.t. geometric
   properties (e.g., radius, length, etc.)

   @param representation  Controls the mesh representation of the contact
                          surface. See
                          @ref contact_surface_discrete_representation
                          "contact surface representation" for more details.

   @returns A vector populated with all detected intersections characterized as
            contact surfaces. The ordering of the results is guaranteed to be
            consistent -- for fixed geometry poses, the results will remain
            the same.  */
  template <typename T1 = T>
  typename std::enable_if_t<scalar_predicate<T1>::is_bool,
                            std::vector<ContactSurface<T>>>
  ComputeContactSurfaces(
      HydroelasticContactRepresentation representation) const;

  /** Reports pairwise intersections and characterizes each non-empty
   intersection as a ContactSurface _where possible_ and as a
   PenetrationAsPointPair where not.

   This method can be thought of as a combination of ComputeContactSurfaces()
   and ComputePointPairPenetration(). For each geometry pair, we attempt to
   compute a ContactSurface. If that fails, rather than throwing, we attempt to
   characterize the contact as a point pair. If that fails, we throw. See the
   documentation of those constituent methods to understand the circumstances
   in which they fail.

   The ordering of the _added_ results is guaranteed to be consistent -- for
   fixed geometry poses, the results will remain the same.

   <h3>Scalar support</h3>

   The scalar support is a combination of the scalar support offered by
   ComputeContactSurfaces() and ComputePointPairPenetration(). This method
   supports double and AutoDiffXd to the extent that those constituent methods
   do, but does not support Expression.

   @param representation    Controls the mesh representation of the contact
                            surface. See
                            @ref contact_surface_discrete_representation
                            "contact surface representation" for more details.
   @param[out] surfaces     The vector that contact surfaces will be added to.
                            The vector will _not_ be cleared.
   @param[out] point_pairs  The vector that fall back point pair data will be
                            added to. The vector will _not_ be cleared.
   @pre Neither `surfaces` nor `point_pairs` is nullptr.
   @throws std::exception for the reasons described in ComputeContactSurfaces()
                          and ComputePointPairPenetration().
   @note The `surfaces` and `point_pairs` are output pointers in C++, but are
   return values in the Python bindings. */
  template <typename T1 = T>
  typename std::enable_if_t<scalar_predicate<T1>::is_bool, void>
  ComputeContactSurfacesWithFallback(
      HydroelasticContactRepresentation representation,
      std::vector<ContactSurface<T>>* surfaces,
      std::vector<PenetrationAsPointPair<T>>* point_pairs) const;

  /** Reports contact information among all deformable geometries. This function
   only supports double as the scalar type.
   @param[out] deformable_contact
     Contains all deformable contact data on output. Any data passed in is
     cleared before the computation.
   @pre deformable_contact != nullptr.
   @experimental */
  template <typename T1 = T>
  typename std::enable_if_t<std::is_same_v<T1, double>, void>
  ComputeDeformableContact(
      internal::DeformableContact<T>* deformable_contact) const;

  /** Applies a conservative culling mechanism to create a subset of all
   possible geometry pairs based on non-zero intersections. A geometry pair
   that is *absent* from the results is either a) culled by collision filters or
   b) *known* to be separated. The caller is responsible for confirming that
   the remaining, unculled geometry pairs are *actually* in collision.

   @returns A vector populated with collision pair candidates (the order will
            remain constant for a fixed population but can change as geometry
            ids are added/removed).  */
  std::vector<SortedPair<GeometryId>> FindCollisionCandidates() const;

  /** Reports true if there are _any_ collisions between unfiltered pairs in the
   world.
   @warning For Mesh shapes, their convex hulls are used in this query. It is
            *not* computationally efficient or particularly accurate.  */
  bool HasCollisions() const;

  //@}

  //---------------------------------------------------------------------------
  // TODO(DamrongGuoy): Write a better documentation for Signed Distance
  // Queries.
  /**
   @anchor signed_distance_query
   @name                   Signed Distance Queries

   These queries provide the signed distance between two objects. Each query
   has a specific definition of the signed distance being positive, negative,
   or zero associated with some notions of being outside, inside, or on
   the boundary.

   These queries provide bookkeeping data like geometry id(s) of the geometries
   involved and the important locations on the boundaries of these geometries.

   The signed distance function is a continuous function. Its partial
   derivatives are continuous almost everywhere.
  */
  //@{

  // TODO(DamrongGuoy): Refactor documentation of
  // ComputeSignedDistancePairwiseClosestPoints(). Move the common sections
  // into Signed Distance Queries.
  /**
   Computes the signed distance together with the nearest points across all
   pairs of geometries in the world. Reports both the separating geometries
   and penetrating geometries.

   This query provides φ(A, B), the signed distance between two objects A and B.

   If the objects do not overlap (i.e., A ⋂ B = ∅), φ > 0 and represents the
   minimal distance between the two objects. More formally:
   φ = min(|Aₚ - Bₚ|)
   ∀ Aₚ ∈ A and Bₚ ∈ B.
   @note The pair (Aₚ, Bₚ) is a "witness" of the distance.
   The pair need not be unique (think of two parallel planes).

   If the objects touch or overlap (i.e., A ⋂ B ≠ ∅), φ ≤ 0 and can be
   interpreted as the negative penetration depth. It is the smallest length of
   the vector v, such that by shifting one object along that vector relative to
   the other, the two objects will no longer be overlapping. More formally,
   φ(A, B) = -min |v|.
   s.t (Tᵥ · A) ⋂ B = ∅
   where Tᵥ is a rigid transformation that displaces A by the vector v, namely
   Tᵥ · A = {u + v | ∀ u ∈ A}.
   By implication, there exist points Aₚ and Bₚ on the surfaces of objects A and
   B, respectively, such that Aₚ + v = Bₚ, Aₚ ∈ A ∩ B, Bₚ ∈ A ∩ B. These points
   are the witnesses to the penetration.

   This method is affected by collision filtering; geometry pairs that
   have been filtered will not produce signed distance query results.

   For a geometry pair (A, B), the returned results will always be reported in
   a fixed order (e.g., always (A, B) and never (B, A)). The _basis_ for the
   ordering is arbitrary (and therefore undocumented), but guaranteed to be
   fixed and repeatable.

   Notice that this is an O(N²) operation, where N
   is the number of geometries remaining in the world after applying collision
   filter. We report the distance between dynamic objects, and between dynamic
   and anchored objects. We DO NOT report the distance between two anchored
   objects.

   <h3>Using maximum distance</h3>

   While the algorithm generally has O(N²) complexity in time and space, that
   can be reduced by the judicious use of the `max_distance` parameter. If
   `φ(A, B) > max_distance`, the pair (A, B) will not be included in the
   results (making it O(M²) in space where M < N). Furthermore, the broadphase
   culling algorithm can exploit `max_distance` to *cheaply* eliminate pairs of
   geometry that are "obviously" too far (likewise reducing the time
   complexity).

   Passing `max_distance = 0` is conceptually related to calling
   HasCollisions(). If contact is sparse (very few actually contacting geometry
   pairs), the two invocations will be quite similar in cost. However, the
   distinction between the two is that *this* method would have to include *all*
   pairs that satisfy `φ(A, B) <= 0`, whereas HasCollisions() stops at the
   first. So, the more actually colliding geometry pairs there are, the bigger
   the difference in cost between the two approaches.

   @anchor query_object_compute_pairwise_distance_table
   <h3>Characterizing the returned values</h3>

   As discussed in the
   @ref query_object_precision_methodology "class's documentation", this table
   documents the support given by this query for pairs of geometry types and
   scalar. See the description in the link for details on how to interpret the
   table results. The query is symmetric with respect to shape *ordering*, the
   pair (ShapeA, ShapeB) will be the same as (ShapeB, ShapeA), so we only fill
   in half the table.

   |           |   %Box  | %Capsule | %Convex | %Cylinder | %Ellipsoid | %HalfSpace |  %Mesh  | %Sphere |
   | --------: | :-----: | :------: | :-----: | :-------: | :--------: | :--------: | :-----: | :-----: |
   | Box       |  4e-15  |  ░░░░░░  |  ░░░░░  |  ░░░░░░░  |   ░░░░░░   |   ░░░░░░   |  ░░░░░  |  ░░░░░  |
   | Capsule   |  3e-6   |   2e-5   |  ░░░░░  |  ░░░░░░░  |   ░░░░░░   |   ░░░░░░   |  ░░░░░  |  ░░░░░  |
   | Convex    |  3e-15  |   2e-5   |  3e-15  |  ░░░░░░░  |   ░░░░░░   |   ░░░░░░   |  ░░░░░  |  ░░░░░  |
   | Cylinder  |  6e-6   |   1e-5   |   6e-6  |   2e-5    |   ░░░░░░   |   ░░░░░░   |  ░░░░░  |  ░░░░░  |
   | Ellipsoid |  9e-6   |   5e-6   |   9e-6  |   5e-5    |    2e-5    |   ░░░░░░   |  ░░░░░  |  ░░░░░  |
   | HalfSpace | throwsᵃ |  throwsᵃ | throwsᵃ |  throwsᵃ  |  throwsᵃ   |   throwsᵃ  |  ░░░░░  |  ░░░░░  |
   | Mesh      |    ᶜ    |    ᶜ     |    ᶜ    |     ᶜ     |      ᶜ     |   throwsᵃ  |    ᶜ    |  ░░░░░  |
   | Sphere    |  3e-15  |  6e-15   |   3e-6  |   5e-15   |    4e-5    |    3e-15   |    ᶜ    |  6e-15  |
   __*Table 4*__: Worst observed error (in m) for 2mm penetration/separation
   between geometries approximately 20cm in size for `T` = `double`.

   |           |   %Box  | %Capsule | %Convex | %Cylinder | %Ellipsoid | %HalfSpace |  %Mesh  | %Sphere |
   | --------: | :-----: | :------: | :-----: | :-------: | :--------: | :--------: | :-----: | :-----: |
   | Box       | throwsᵇ |  ░░░░░░  |  ░░░░░  |  ░░░░░░░  |   ░░░░░░   |   ░░░░░░   |  ░░░░░  |  ░░░░░  |
   | Capsule   | throwsᵇ |  throwsᵇ |  ░░░░░  |  ░░░░░░░  |   ░░░░░░   |   ░░░░░░   |  ░░░░░  |  ░░░░░  |
   | Convex    | throwsᵇ |  throwsᵇ | throwsᵇ |  ░░░░░░░  |   ░░░░░░   |   ░░░░░░   |  ░░░░░  |  ░░░░░  |
   | Cylinder  | throwsᵇ |  throwsᵇ | throwsᵇ |  throwsᵇ  |   ░░░░░░   |   ░░░░░░   |  ░░░░░  |  ░░░░░  |
   | Ellipsoid | throwsᵇ |  throwsᵇ | throwsᵇ |  throwsᵇ  |  throwsᵇ   |   ░░░░░░   |  ░░░░░  |  ░░░░░  |
   | HalfSpace | throwsᵃ |  throwsᵃ | throwsᵃ |  throwsᵃ  |  throwsᵃ   |   throwsᵃ  |  ░░░░░  |  ░░░░░  |
   | Mesh      |    ᶜ    |    ᶜ     |    ᶜ    |     ᶜ     |      ᶜ     |      ᵃ     |    ᶜ    |  ░░░░░  |
   | Sphere    |  2e-15  |  throwsᵇ | throwsᵇ |  throwsᵇ  |  throwsᵇ   |    2e-15   |    ᶜ    |  5e-15  |
   __*Table 5*__: Worst observed error (in m) for 2mm penetration/separation
   between geometries approximately 20cm in size for `T` =
   @ref drake::AutoDiffXd "AutoDiffXd".

   |           |   %Box  | %Capsule | %Convex | %Cylinder | %Ellipsoid | %HalfSpace |  %Mesh  | %Sphere |
   | --------: | :-----: | :------: | :-----: | :-------: | :--------: | :--------: | :-----: | :-----: |
   | Box       | throwsᵈ |  ░░░░░░  |  ░░░░░  |  ░░░░░░░  |   ░░░░░░   |   ░░░░░░   |  ░░░░░  |  ░░░░░  |
   | Capsule   | throwsᵈ | throwsᵈ  |  ░░░░░  |  ░░░░░░░  |   ░░░░░░   |   ░░░░░░   |  ░░░░░  |  ░░░░░  |
   | Convex    | throwsᵈ | throwsᵈ  | throwsᵈ |  ░░░░░░░  |   ░░░░░░   |   ░░░░░░   |  ░░░░░  |  ░░░░░  |
   | Cylinder  | throwsᵈ | throwsᵈ  | throwsᵈ |  throwsᵈ  |   ░░░░░░   |   ░░░░░░   |  ░░░░░  |  ░░░░░  |
   | Ellipsoid | throwsᵈ | throwsᵈ  | throwsᵈ |  throwsᵈ  |   throwsᵈ  |   ░░░░░░   |  ░░░░░  |  ░░░░░  |
   | HalfSpace | throwsᵈ | throwsᵈ  | throwsᵈ |  throwsᵈ  |   throwsᵈ  |   throwsᵈ  |  ░░░░░  |  ░░░░░  |
   | Mesh      |    ᵇ    |    ᵇ     |    ᵇ    |     ᵇ     |      ᵇ     |     ᵇ      |    ᵇ    |  ░░░░░  |
   | Sphere    | throwsᵈ | throwsᵈ  | throwsᵈ |  throwsᵈ  |   throwsᵈ  |    throwsᵈ  |    ᵇ    | throwsᵈ |
   __*Table 6*__: Support for `T` = @ref drake::symbolic::Expression.

   - ᵃ We don't currently support queries between HalfSpace and any other shape
       (except for Sphere).
   - ᵇ These results are simply not supported for
       `T` = @ref drake::AutoDiffXd "AutoDiffXd" at this time.
   - ᶜ Meshes are represented by the *convex* hull of the mesh, therefore the
       results for Mesh are the same as for Convex.
   - ᵈ These results are simply not supported for
       `T` = @ref drake::symbolic::Expression at this time.

   <!-- Note to developers: the tests that support the assertions here are
   located in distance_to_shape_characterize_test.cc. The values in this
   table should be reflected in the expected values there.  -->

  <!-- Numerous values show deviation at 1e-6. This is a hard-coded iteration
   limit in proximity engine. Consider exposing that parameter to the query
   so that the user can choose to improve the answer.  -->

   <!-- TODO(SeanCurtis-TRI): Support queries of halfspace-A, where A is _not_ a
   halfspace. See https://github.com/RobotLocomotion/drake/issues/10905 -->

   @param max_distance  The maximum distance at which distance data is reported.

   @returns The signed distance (and supporting data) for all unfiltered
            geometry pairs whose distance is less than or equal to
            `max_distance`.
   @throws std::exception as indicated in the table above.
   @warning For Mesh shapes, their convex hulls are used in this query. It is
            *not* computationally efficient or particularly accurate.  */
  std::vector<SignedDistancePair<T>> ComputeSignedDistancePairwiseClosestPoints(
      const double max_distance =
          std::numeric_limits<double>::infinity()) const;

  /** A variant of ComputeSignedDistancePairwiseClosestPoints() which computes
   the signed distance (and witnesses) between a specific pair of geometries
   indicated by id. This function has the same restrictions on scalar report
   as ComputeSignedDistancePairwiseClosestPoints().

   <h3>Characterizing the returned values</h3>

   This method merely exercises the same mechanisms as
   ComputeSignedDistancePairwiseClosestPoints() for evaluating signed distance.
   Refer to @ref query_object_compute_pairwise_distance_table
   "the table for ComputeSignedDistancePairwiseClosestPoints()" for details.

   @throws std::exception if either geometry id is invalid (e.g., doesn't refer
                          to an existing geometry, lacking proximity role,
                          etc.), the pair (A, B) has been marked as filtered, or
                          otherwise unsupported as indicated by the the scalar
                          support table.
   @warning For Mesh shapes, their convex hulls are used in this query. It is
            *not* computationally efficient or particularly accurate.  */
  SignedDistancePair<T> ComputeSignedDistancePairClosestPoints(
      GeometryId geometry_id_A, GeometryId geometry_id_B) const;

  // TODO(DamrongGuoy): Improve and refactor documentation of
  // ComputeSignedDistanceToPoint(). Move the common sections into Signed
  // Distance Queries. Update documentation as we add more functionality.
  // Right now it only supports spheres and boxes.
  /**
   Computes the signed distances and gradients to a query point from each
   geometry in the scene.

   This query provides φᵢ(p), φᵢ:ℝ³→ℝ, the signed distance to the position
   p of a query point from geometry Gᵢ in the scene.  It returns an array of
   the signed distances from all geometries.

   Optionally you can specify a threshold distance that will filter out any
   object beyond the threshold. By default, we report distances from the query
   point to every object.

   This query also provides the gradient vector ∇φᵢ(p) of the signed distance
   function from geometry Gᵢ. Note that, in general, if p is outside Gᵢ, then
   ∇φᵢ(p) equals the unit vector in the direction from the nearest point Nᵢ on
   Gᵢ's surface to p. If p is inside Gᵢ, then ∇φᵢ(p) is in the direction from
   p to Nᵢ. This observation is written formally as:

   ∇φᵢ(p) = (p - Nᵢ)/|p - Nᵢ| if p is outside Gᵢ

   ∇φᵢ(p) = -(p - Nᵢ)/|p - Nᵢ| if p is inside Gᵢ

   Note that ∇φᵢ(p) is also defined on Gᵢ's surface, but we cannot use the
   above formula.

   <h3>Characterizing the returned values</h3>

   This table is a variant of that described in this
   @ref query_object_precision_methodology "class's documentation". The query
   evaluates signed distance between *one* shape and a point (in contrast to
   other queries which involve two shapes). Therefore, we don't need a matrix
   of shape pairs, but a list of shapes. Otherwise, the methodology is the same
   as described, with the point being represented as a zero-radius sphere.

   |   Scalar   |   %Box  | %Capsule | %Convex | %Cylinder | %Ellipsoid | %HalfSpace |  %Mesh  | %Sphere |
   | :----: | :-----: | :------: | :-----: | :-------: | :--------: | :--------: | :-----: | :-----: |
   |   double   |  2e-15  |   4e-15  |    ᵃ    |   3e-15   |    3e-5ᵇ   |    5e-15   |    ᵃ    |  4e-15  |
   | AutoDiffXd |  1e-15  |   4e-15  |    ᵃ    |     ᵃ     |      ᵃ     |    5e-15   |    ᵃ    |  3e-15  |
   | Expression |   ᵃ     |    ᵃ     |    ᵃ    |     ᵃ     |      ᵃ     |      ᵃ     |    ᵃ    |    ᵃ    |
   __*Table 7*__: Worst observed error (in m) for 2mm penetration/separation
   between geometry approximately 20cm in size and a point.

   - ᵃ Unsupported geometry/scalar combinations are simply ignored; no results
       are reported for that geometry.
   - ᵇ This uses an *iterative* algorithm which introduces a relatively large
       and variable error. For example, as the eccentricity of the ellipsoid
       increases, this error may get worse. It also depends on the location of
       the projection of the query point on the ellipsoid; the closer that point
       is to the high curvature area, the bigger the effect. It is not
       immediately clear how much worse the answer will get.

   @note For a sphere G, the signed distance function φᵢ(p) has an undefined
   gradient vector at the center of the sphere--every point on the sphere's
   surface has the same distance to the center.  In this case, we will assign
   ∇φᵢ(p) the unit vector Gx (x-directional vector of G's frame) expressed
   in World frame.

   @note For a box, at a point p on an edge or a corner of the box, the signed
   distance function φᵢ(p) has an undefined gradient vector.  In this case, we
   will assign a unit vector in the direction of the average of the outward
   face unit normals of the incident faces of the edge or the corner.
   A point p is considered being on a face, or an edge, or a corner of the
   box if it lies within a certain tolerance from them.

   @note For a box B, if a point p is inside the box, and it is equidistant to
   multiple nearest faces, the signed distance function φᵢ(p) at p will have
   an undefined gradient vector. There is a nearest point candidate associated
   with each nearest face. In this case, we arbitrarily pick the point Nᵢ
   associated with one of the nearest faces.  Please note that, due to the
   possible round off error arising from applying a pose X_WG to B, there is no
   guarantee which of the nearest faces will be used.

   @note The signed distance function is a continuous function with respect to
   the position of the query point, but its gradient vector field may
   not be continuous. Specifically at a position equidistant to multiple
   nearest points, its gradient vector field is not continuous.

   @note For a convex object, outside the object at positive distance from
   the boundary, the signed distance function is smooth (having continuous
   first-order partial derivatives).

   @param[in] p_WQ            Position of a query point Q in world frame W.
   @param[in] threshold       We ignore any object beyond this distance.
                              By default, it is infinity, so we report
                              distances from the query point to every object.
   @retval signed_distances   A vector populated with per-object signed distance
                              values (and supporting data) for every supported
                              geometry as shown in the table. See
                              SignedDistanceToPoint. */
  std::vector<SignedDistanceToPoint<T>>
  ComputeSignedDistanceToPoint(const Vector3<T> &p_WQ,
                               const double threshold
                               = std::numeric_limits<double>::infinity()) const;
  //@}


  //---------------------------------------------------------------------------
  /**
   @anchor render_queries
   @name                Render Queries

   The methods support queries along the lines of "What do I see?" They support
   simulation of sensors. External entities define a sensor camera -- its
   extrinsic and intrinsic properties and %QueryObject renders into the
   provided image.

   <!-- TODO(SeanCurtis-TRI): Currently, pose is requested as a transform of
   double. This puts the burden on the caller to be compatible. Provide
   specializations for AutoDiff and symbolic (the former extracts a
   double-valued transform and the latter throws). -->
   */
  //@{

  /** Renders an RGB image for the given `camera` posed with respect to the
   indicated parent frame P.

   @param camera                The camera to render from.
   @param parent_frame          The id for the camera's parent frame.
   @param X_PC                  The pose of the camera body in the parent frame.
   @param[out] color_image_out  The rendered color image. */
  void RenderColorImage(const render::ColorRenderCamera& camera,
                        FrameId parent_frame, const math::RigidTransformd& X_PC,
                        systems::sensors::ImageRgba8U* color_image_out) const;

  /** Renders a depth image for the given `camera` posed with respect to the
   indicated parent frame P.

   In contrast to the other rendering methods, rendering depth images doesn't
   provide the option to display the window; generally, basic depth images are
   not readily communicative to humans.

   @param camera                The camera to render from.
   @param parent_frame          The id for the camera's parent frame.
   @param X_PC                  The pose of the camera body in the parent frame.
   @param[out] depth_image_out  The rendered depth image. */
  void RenderDepthImage(const render::DepthRenderCamera& camera,
                        FrameId parent_frame, const math::RigidTransformd& X_PC,
                        systems::sensors::ImageDepth32F* depth_image_out) const;

  /** Renders a label image for the given `camera` posed with respect to the
   indicated parent frame P.

   @param camera                The camera to render from.
   @param parent_frame          The id for the camera's parent frame.
   @param X_PC                  The pose of the camera body in the parent frame.
   @param[out] label_image_out  The rendered label image. */
  void RenderLabelImage(const render::ColorRenderCamera& camera,
                        FrameId parent_frame, const math::RigidTransformd& X_PC,
                        systems::sensors::ImageLabel16I* label_image_out) const;


  /** Returns the named render engine, if it exists. The RenderEngine is
   guaranteed to be up to date w.r.t. the poses and data in the context. */
  const render::RenderEngine* GetRenderEngineByName(
      const std::string& name) const;

  //@}

 private:
  // SceneGraph is the only class that may call set().
  friend class SceneGraph<T>;
  // Convenience class for testing.
  friend class QueryObjectTest;

  // Access the GeometryState associated with this QueryObject.
  // @pre ThrowIfNotCallable() has been invoked prior to this.
  const GeometryState<T>& geometry_state() const;

  // Sets the query object to be *live*. That means the `context` and
  // `scene_graph` cannot be null.
  void set(const systems::Context<T>* context,
           const SceneGraph<T>* scene_graph) {
    DRAKE_DEMAND(context != nullptr);
    DRAKE_DEMAND(scene_graph != nullptr);
    state_.reset();
    context_ = context;
    scene_graph_ = scene_graph;
    inspector_.set(&geometry_state());
  }

  // Update poses for all rigid (non-deformable) geometries. This method does no
  // work if this is a "baked" query object (see class docs for discussion).
  void FullPoseUpdate() const {
    // TODO(SeanCurtis-TRI): Modify this when the cache system is in place.
    //  Ideally, QueryObject should never have to invoke any explicit state
    //  updating call at all. It should simply request the geometry state and
    //  rely on the fact that it will always get an up-to-date version.
    if (scene_graph_) scene_graph_->FullPoseUpdate(*context_);
  }

  // Update configurations for all deformable geometries. This method does no
  // work if this is a "baked" query object (see class docs for discussion).
  void FullConfigurationUpdate() const {
    // TODO(SeanCurtis-TRI): Modify this when the cache system is in place.
    //  Ideally, QueryObject should never have to invoke any explicit state
    //  updating call at all. It should simply request the geometry state and
    //  rely on the fact that it will always get an up-to-date version.
    if (scene_graph_) scene_graph_->FullConfigurationUpdate(*context_);
  }

  // Update poses for all rigid (non-deformable) geometries and configurations
  // for all deformable geometries. This method does no work if this is a
  // "baked" query object (see class docs for discussion).
  void FullPoseAndConfigurationUpdate() const {
    FullPoseUpdate();
    FullConfigurationUpdate();
  }

  // Reports true if this object is configured so that it can support a query.
  bool is_callable() const {
    const bool live_condition = context_ != nullptr && scene_graph_ != nullptr;
    const bool baked_condition = state_ != nullptr;
    // I.e., only one of the two conditions can be satisfied.
    return live_condition != baked_condition;
  }

  // Reports true if this object is in default configuration (not callable).
  bool is_default() const {
    return context_ == nullptr && scene_graph_ == nullptr && state_ == nullptr;
  }

  // Reports if the object can be copied; it must either be callable or default.
  bool is_copyable() const {
    return is_callable() || is_default();
  }

  // Throws an exception if the QueryObject is neither "live" nor "baked" (see
  // class docs for discussion).
  void ThrowIfNotCallable() const {
    if (!is_callable()) {
      throw std::runtime_error(
          "Attempting to perform query on invalid QueryObject.");
    }
  }

  // TODO(SeanCurtis-TRI): Consider an alternate formulation. This stores
  // pointers to context and systems which raise some red flags in order to
  // enable other systems to evaluate queries without having a copy of the
  // geometry system or its context.
  //
  // Alternatively, this could store the *index* of the system in its parent
  // diagram. The context shares the same index in the parent diagram context.
  // Then the LeafSystem desiring to perform a query would pass itself and its
  // own context in (along with the query parameters). The QueryObject would
  // use those and the index to get the SceneGraph and Context.
  //
  // Several issues:
  //  1. Leads to a clunky API (passing self and context into *every* query).
  //  2. The index value would be insufficient if the SceneGraph were buried
  //     in a diagram with its query object port exported in the diagram.
  // This is documented for future consideration, and should not necessarily be
  // interpreted as a guaranteed task.

  // The contents of the "live" query object. It has pointers to the system and
  // context from which it spawned. It uses these to compute geometry queries
  // on the current context (fully-dependent on context). These pointers must
  // _both_ be non-null for a live QueryObject and both be null for a baked
  // QueryObject.
  const systems::Context<T>* context_{nullptr};
  const SceneGraph<T>* scene_graph_{nullptr};

  SceneGraphInspector<T> inspector_;

  // When a QueryObject is copied to a "baked" version, it contains a fully
  // updated GeometryState. Copies of bakes all share the same version.
  std::shared_ptr<const GeometryState<T>> state_{};
};

}  // namespace geometry
}  // namespace drake