Drake
ProximityEngine< T > Class Template Reference

The underlying engine for performing geometric proximity queries. More...

#include <drake/geometry/proximity_engine.h>

Public Member Functions

 ProximityEngine ()
 
 ~ProximityEngine ()
 
 ProximityEngine (const ProximityEngine &other)
 Construct a deep copy of the provided other engine. More...
 
ProximityEngineoperator= (const ProximityEngine &other)
 Set this engine to be a deep copy of the other engine. More...
 
 ProximityEngine (ProximityEngine &&other) noexcept
 Construct an engine by moving the data of a source engine. More...
 
ProximityEngineoperator= (ProximityEngine &&other) noexcept
 Move assign a source engine to this engine. More...
 
std::unique_ptr< ProximityEngine< AutoDiffXd > > ToAutoDiffXd () const
 Returns an independent copy of this engine templated on the AutoDiffXd scalar type. More...
 
void UpdateWorldPoses (const std::vector< Isometry3< T >> &X_WG)
 Updates the poses for all of the dynamic geometries in the engine. More...
 
template<typename T >
 ProximityEngine (ProximityEngine< T >::Impl *impl)
 
template<typename T >
 ProximityEngine (const ProximityEngine< T > &other)
 
template<typename T >
 ProximityEngine (ProximityEngine< T > &&other) noexcept
 
Topology management
ProximityIndex AddDynamicGeometry (const Shape &shape, GeometryIndex index)
 Adds the given shape to the engine's dynamic geometry. More...
 
ProximityIndex AddAnchoredGeometry (const Shape &shape, const Isometry3< double > &X_WG, GeometryIndex index)
 Adds the given shape to the engine's anchored geometry. More...
 
int num_geometries () const
 Reports the total number of geometries in the engine – dynamic and anchored (spanning all sources). More...
 
int num_dynamic () const
 Reports the number of dynamic geometries (spanning all sources). More...
 
int num_anchored () const
 Reports the number of anchored geometries (spanning all sources). More...
 
void set_distance_tolerance (double tol)
 The distance (signed/unsigned/penetration distance) is generally computed from an iterative process. More...
 
double distance_tolerance () const
 
Signed Distance Queries

See Signed Distance Query for more details.

std::vector< SignedDistancePair< double > > ComputeSignedDistancePairwiseClosestPoints (const std::vector< GeometryId > &geometry_map) const
 Determines all the closest points between any pair of bodies/elements. More...
 
Collision Queries

These queries detect collisions between geometry.

Two geometries collide if they overlap each other and are not explicitly excluded through collision filtering. These algorithms find those colliding cases, characterize them, and report the essential characteristics of that collision.

std::vector< PenetrationAsPointPair< double > > ComputePointPairPenetration (const std::vector< GeometryId > &geometry_map) const
 Computes the penetrations across all pairs of geometries in the world. More...
 
Collision filters

This interface provides the mechanism through which pairs of geometries are removed from the "candidate pair set" for collision detection.

See Scene Graph Collision Filtering for more details.

void ExcludeCollisionsWithin (const std::unordered_set< GeometryIndex > &dynamic, const std::unordered_set< GeometryIndex > &anchored)
 Excludes geometry pairs from collision evaluation by updating the candidate pair set C = C - P, where P = {(gᵢ, gⱼ)}, ∀ gᵢ, gⱼ ∈ G and G = dynamic ⋃ anchored = {g₀, g₁, ..., gₙ}. More...
 
void ExcludeCollisionsBetween (const std::unordered_set< GeometryIndex > &dynamic1, const std::unordered_set< GeometryIndex > &anchored1, const std::unordered_set< GeometryIndex > &dynamic2, const std::unordered_set< GeometryIndex > &anchored2)
 Excludes geometry pairs from collision evaluation by updating the candidate pair set C = C - P, where P = {(a, b)}, ∀ a ∈ A, b ∈ B and A = dynamic1 ⋃ anchored1 = {a₀, a₁, ..., aₘ} and B = dynamic2 ⋃ anchored2 = {b₀, b₁, ..., bₙ}. More...
 

Friends

class GeometryStateCollisionFilterAttorney
 
template<typename >
class ProximityEngine
 
class ProximityEngineTester
 

Detailed Description

template<typename T>
class drake::geometry::internal::ProximityEngine< T >

The underlying engine for performing geometric proximity queries.

It owns the geometry instances and, once it has been provided with the poses of the geometry, it provides geometric queries on that geometry.

Proximity queries span a range of types, including:

  • penetration
  • distance
  • ray-intersection
Template Parameters
TThe scalar type. Must be a valid Eigen scalar.

Instantiated templates for the following kinds of T's are provided:

  • double
  • AutoDiffXd

They are already available to link against in the containing library. No other values for T are currently supported.

Constructor & Destructor Documentation

◆ ProximityEngine() [1/6]

◆ ~ProximityEngine()

◆ ProximityEngine() [2/6]

ProximityEngine ( const ProximityEngine< T > &  other)

Construct a deep copy of the provided other engine.

◆ ProximityEngine() [3/6]

ProximityEngine ( ProximityEngine< T > &&  other)
noexcept

Construct an engine by moving the data of a source engine.

The source engine will be returned to its default-initialized state.

◆ ProximityEngine() [4/6]

ProximityEngine ( ProximityEngine< T >::Impl *  impl)

◆ ProximityEngine() [5/6]

ProximityEngine ( const ProximityEngine< T > &  other)

◆ ProximityEngine() [6/6]

ProximityEngine ( ProximityEngine< T > &&  other)
noexcept

Member Function Documentation

◆ AddAnchoredGeometry()

ProximityIndex AddAnchoredGeometry ( const Shape shape,
const Isometry3< double > &  X_WG,
GeometryIndex  index 
)

Adds the given shape to the engine's anchored geometry.

Parameters
shapeThe shape to add.
X_WGThe pose of the shape in the world frame.
indexThe index of the geometry in SceneGraph to which this shape belongs.
Returns
the index of the added shape in the proximity engine.

◆ AddDynamicGeometry()

ProximityIndex AddDynamicGeometry ( const Shape shape,
GeometryIndex  index 
)

Adds the given shape to the engine's dynamic geometry.

Parameters
shapeThe shape to add.
indexThe index of the geometry in SceneGraph to which this shape belongs.
Returns
the index of the added shape in the proximity engine.

◆ ComputePointPairPenetration()

std::vector< PenetrationAsPointPair< double > > ComputePointPairPenetration ( const std::vector< GeometryId > &  geometry_map) const

Computes the penetrations across all pairs of geometries in the world.

Only reports results for penetrating geometries; if two geometries are separated, there will be no result for that pair.

The penetrations are characterized by pairs of points (providing some measure of the penetration "depth" of the two objects – but not the overlapping volume.

This method is affected by collision filtering; geometry pairs that have been filtered will not produce contacts, even if their collision geometry is penetrating.

For two penetrating geometries g₁ and g₂, it is guaranteed that they will map to id_A and id_B in a fixed, repeatable manner.

Parameters
[in]geometry_mapA map from geometry index to the corresponding global geometry identifier.
Returns
A vector populated with all detected penetrations characterized as point pairs.

◆ ComputeSignedDistancePairwiseClosestPoints()

std::vector< SignedDistancePair< double > > ComputeSignedDistancePairwiseClosestPoints ( const std::vector< GeometryId > &  geometry_map) const

Determines all the closest points between any pair of bodies/elements.

This function returns the signed distance between all valid pairs of geometries. A valid pair consists of either two dynamic geometries or a dynamic geometry and an anchored geometry. It never includes two anchored geometries. The order and size of the returned vector are invariant when the poses of the objects are changed.

Parameters
[in]geometry_mapA map from geometry index to the corresponding global geometry identifier.
Return values
signed_distancesA vector populated with per-object-pair signed distance values (and supporting data). Note: For a geometry pair (A, B), the supporting data 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.

◆ distance_tolerance()

double distance_tolerance ( ) const

◆ ExcludeCollisionsBetween()

void ExcludeCollisionsBetween ( const std::unordered_set< GeometryIndex > &  dynamic1,
const std::unordered_set< GeometryIndex > &  anchored1,
const std::unordered_set< GeometryIndex > &  dynamic2,
const std::unordered_set< GeometryIndex > &  anchored2 
)

Excludes geometry pairs from collision evaluation by updating the candidate pair set C = C - P, where P = {(a, b)}, ∀ a ∈ A, b ∈ B and A = dynamic1 ⋃ anchored1 = {a₀, a₁, ..., aₘ} and B = dynamic2 ⋃ anchored2 = {b₀, b₁, ..., bₙ}.

This does not preclude collisions between members of the same set.

◆ ExcludeCollisionsWithin()

void ExcludeCollisionsWithin ( const std::unordered_set< GeometryIndex > &  dynamic,
const std::unordered_set< GeometryIndex > &  anchored 
)

Excludes geometry pairs from collision evaluation by updating the candidate pair set C = C - P, where P = {(gᵢ, gⱼ)}, ∀ gᵢ, gⱼ ∈ G and G = dynamic ⋃ anchored = {g₀, g₁, ..., gₙ}.

Parameters
[in]dynamicThe set of geometry indices for dynamic geometries for which no collisions can be reported.
[in]anchoredThe set of geometry indices for anchored geometries for which no collisions can be reported.

◆ num_anchored()

int num_anchored ( ) const

Reports the number of anchored geometries (spanning all sources).

◆ num_dynamic()

int num_dynamic ( ) const

Reports the number of dynamic geometries (spanning all sources).

◆ num_geometries()

int num_geometries ( ) const

Reports the total number of geometries in the engine – dynamic and anchored (spanning all sources).

◆ operator=() [1/2]

ProximityEngine& operator= ( const ProximityEngine< T > &  other)

Set this engine to be a deep copy of the other engine.

◆ operator=() [2/2]

ProximityEngine& operator= ( ProximityEngine< T > &&  other)
noexcept

Move assign a source engine to this engine.

The source engine will be returned to its default-initialized state.

◆ set_distance_tolerance()

void set_distance_tolerance ( double  tol)

The distance (signed/unsigned/penetration distance) is generally computed from an iterative process.

The distance_tolerance determines when the iterative process will terminate. As a rule of rule of thumb, one can generally assume that the answer will be within 10 * tol to the true answer.

◆ ToAutoDiffXd()

std::unique_ptr< ProximityEngine< AutoDiffXd > > ToAutoDiffXd ( ) const

Returns an independent copy of this engine templated on the AutoDiffXd scalar type.

If the engine is already an AutoDiffXd engine, it is equivalent to using the copy constructor to create a duplicate on the heap.

◆ UpdateWorldPoses()

void UpdateWorldPoses ( const std::vector< Isometry3< T >> &  X_WG)

Updates the poses for all of the dynamic geometries in the engine.

It is an invariant that every registered dynamic geometry, across all geometry sources, has a unique index that lies in the range [0, num_dynamic() - 1]. Therefore, X_WG should have size equal to num_dynamics() and any other length will cause program failure. The iᵗʰ entry contains the pose for the geometry whose GeometryIndex value is i.

Parameters
X_WGThe poses of each geometry G measured and expressed in the world frame W.

Friends And Related Function Documentation

◆ GeometryStateCollisionFilterAttorney

friend class GeometryStateCollisionFilterAttorney
friend

◆ ProximityEngine

friend class ProximityEngine
friend

◆ ProximityEngineTester

friend class ProximityEngineTester
friend

The documentation for this class was generated from the following files: