pydrake.geometry

class pydrake.geometry.Box

Bases: pydrake.geometry.Shape

Definition of a box. The box is centered on the origin of its canonical frame with its dimensions aligned with the frame’s axes. The size of the box is given by three sizes.

__init__(self: pydrake.geometry.Box, width: float, depth: float, height: float) → None

Constructs a box with the given width, depth, and height, which specify the box’s dimension along the canonical x-, y-, and z-axes, respectively.

Raises:
  • RuntimeError if width, depth or height are not strictly
  • positive.
depth(self: pydrake.geometry.Box) → float

Returns the box’s dimension along the y axis.

height(self: pydrake.geometry.Box) → float

Returns the box’s dimension along the z axis.

size(self: pydrake.geometry.Box) → numpy.ndarray[numpy.float64[3, 1]]

Returns the box’s dimensions.

width(self: pydrake.geometry.Box) → float

Returns the box’s dimension along the x axis.

class pydrake.geometry.Capsule

Bases: pydrake.geometry.Shape

Definition of a capsule. It is centered in its canonical frame with the length of the capsule parallel with the frame’s z-axis.

__init__(self: pydrake.geometry.Capsule, radius: float, length: float) → None

Constructs a capsule with the given radius and length.

Raises:
  • RuntimeError if radius or length are not strictly
  • positive.
length(self: pydrake.geometry.Capsule) → float
radius(self: pydrake.geometry.Capsule) → float
pydrake.geometry.ConnectDrakeVisualizer(*args, **kwargs)

Overloaded function.

  1. ConnectDrakeVisualizer(builder: pydrake.systems.framework.DiagramBuilder_[float], scene_graph: drake::geometry::SceneGraph<double>, lcm: pydrake.lcm.DrakeLcmInterface = None, role: pydrake.geometry.Role = Role.kIllustration) -> pydrake.systems.lcm.LcmPublisherSystem

Extends a Diagram with the required components to interface with drake_visualizer. This must be called during Diagram building and uses the given builder to add relevant subsystems and connections.

This is a convenience method to simplify some common boilerplate for adding visualization capability to a Diagram. What it does is:

  • adds an initialization event that sends the required load message to set up

the visualizer with the relevant geometry, - adds systems PoseBundleToDrawMessage and LcmPublisherSystem to the Diagram and connects the draw message output to the publisher input, - connects the scene_graph pose bundle output to the PoseBundleToDrawMessage system, and - sets the publishing rate to 1/60 of a second (simulated time).

The visualization mechanism depends on the illustration role (see geometry_roles for details). Specifically, only geometries with the illustration role assigned will be included. The visualization function looks for the following properties in the IllustrationProperties instance.

Group name | Required | Property Name | Property Type | Property

Description | | :——–: | :——: | :———–: | :————-: | :——————- | | phong | no | diffuse | Eigen::Vector4d | The rgba value of the object surface |

See MakePhongIllustrationProperties() to facilitate making a compliant set of illustration properties.

You can then connect source output ports for visualization like this:

builder->Connect(pose_output_port,
scene_graph.get_source_pose_port(source_id));

Note

The initialization event occurs when Simulator::Initialize() is called (explicitly or implicitly at the start of a simulation). If you aren’t going to be using a Simulator, use DispatchLoadMessage() to send the message yourself.

Parameter builder:
The diagram builder being used to construct the Diagram.
Parameter scene_graph:
The System in builder containing the geometry to be visualized.
Parameter lcm:
An optional lcm interface through which lcm messages will be dispatched. Will be allocated internally if none is supplied.
Parameter role:
An optional flag to indicate the role of the geometries to be visualized; defaults to the illustration role.
Precondition:
This method has not been previously called while building the builder’s current Diagram.
Precondition:
The given scene_graph must be contained within the supplied DiagramBuilder.
Returns:the LcmPublisherSystem (in case callers, e.g., need to change the default publishing rate).

See also

geometry::DispatchLoadMessage()

  1. ConnectDrakeVisualizer(builder: pydrake.systems.framework.DiagramBuilder_[float], scene_graph: drake::geometry::SceneGraph<double>, pose_bundle_output_port: pydrake.systems.framework.OutputPort_[float], lcm: pydrake.lcm.DrakeLcmInterface = None, role: pydrake.geometry.Role = Role.kIllustration) -> pydrake.systems.lcm.LcmPublisherSystem

Implements ConnectDrakeVisualizer, but using pose_bundle_output_port to explicitly specify the output port used to get pose bundles for scene_graph. This is required, for instance, when the SceneGraph is inside a Diagram, and the Diagram exports the pose bundle port.

Precondition:
pose_bundle_output_port must be connected directly to the pose_bundle_output_port of scene_graph.

See also

ConnectDrakeVisualizer().

class pydrake.geometry.Convex

Bases: pydrake.geometry.Shape

Support for convex shapes.

__init__(self: pydrake.geometry.Convex, absolute_filename: str, scale: float = 1.0) → None

Constructs a convex shape specification from the file located at the given absolute file path. Optionally uniformly scaled by the given scale factor.

Parameter absolute_filename:
The file name with absolute path. We only support an .obj file with only one polyhedron. We assume that the polyhedron is convex.
Parameter scale:
An optional scale to coordinates.
Raises:
  • RuntimeError if the .obj file doesn’t define a single object. This
  • can happen if it is empty, if there are multiple object-name
  • statements (e.g., “o object_name”), or if there are faces defined
  • outside a single object-name statement.
Raises:
  • RuntimeError if |scale| < 1e-8. Note that a negative scale is
  • considered valid. We want to preclude scales near zero but
  • recognise that scale is a convenience tool for “tweaking” models.
  • 8 orders of magnitude should be plenty without considering
  • revisiting the model itself.
filename(self: pydrake.geometry.Convex) → str
scale(self: pydrake.geometry.Convex) → float
class pydrake.geometry.Cylinder

Bases: pydrake.geometry.Shape

Definition of a cylinder. It is centered in its canonical frame with the length of the cylinder parallel with the frame’s z-axis.

__init__(self: pydrake.geometry.Cylinder, radius: float, length: float) → None

Constructs a cylinder with the given radius and length.

Raises:
  • RuntimeError if radius or length are not strictly
  • positive.
length(self: pydrake.geometry.Cylinder) → float
radius(self: pydrake.geometry.Cylinder) → float
pydrake.geometry.DispatchLoadMessage(scene_graph: drake::geometry::SceneGraph<double>, lcm: pydrake.lcm.DrakeLcmInterface, role: pydrake.geometry.Role = Role.kIllustration) → None

(Advanced) Explicitly dispatches an LCM load message based on the registered geometry. Normally this is done automatically at Simulator initialization. But if you have to do it yourself (likely because you are not using a Simulator), it should be invoked after registration is complete. Typically this is used after ConnectDrakeVisualizer() has been used to add visualization to the Diagram that contains the given scene_graph. The message goes to LCM channel “DRAKE_VIEWER_LOAD_ROBOT”.

See also

geometry::ConnectDrakeVisualizer()

class pydrake.geometry.Ellipsoid

Bases: pydrake.geometry.Shape

Definition of an ellipsoid. It is centered on the origin of its canonical frame with its dimensions aligned with the frame’s axes. The standard equation for the ellipsoid is:

x²/a² + y²/b² + z²/c² = 1, where a,b,c are the lengths of the principal semi-axes of the ellipsoid. The bounding box of the ellipsoid is [-a,a]x[-b,b]x[-c,c].

__init__(self: pydrake.geometry.Ellipsoid, a: float, b: float, c: float) → None

Constructs an ellipsoid with the given lengths of its principal semi-axes.

Raises:RuntimeError if a, b, or c are not strictly positive.
a(self: pydrake.geometry.Ellipsoid) → float
b(self: pydrake.geometry.Ellipsoid) → float
c(self: pydrake.geometry.Ellipsoid) → float
class pydrake.geometry.FrameId

Type used to identify geometry frames in SceneGraph.

__init__

Initialize self. See help(type(self)) for accurate signature.

static get_new_id() → pydrake.geometry.FrameId

Generates a new identifier for this id type. This new identifier will be different from all previous identifiers created. This method does not make any guarantees about the values of ids from successive invocations. This method is guaranteed to be thread safe.

get_value(self: pydrake.geometry.FrameId) → int

Extracts the underlying representation from the identifier. This is considered invalid for invalid ids and is strictly enforced in Debug builds.

is_valid(self: pydrake.geometry.FrameId) → bool

Reports if the id is valid.

pydrake.geometry.FramePoseVector

alias of pydrake.geometry.FramePoseVector_[float]

template pydrake.geometry.FramePoseVector_

Instantiations: FramePoseVector_[float], FramePoseVector_[AutoDiffXd]

class FramePoseVector_[float]

A FrameKinematicsVector is used to report kinematics data for registered frames (identified by unique FrameId values) to SceneGraph. It serves as the basis of FramePoseVector, FrameVelocityVector, and FrameAccelerationVector.

template <typename T>
class MySystem : public LeafSystem<T> {
public:
MySystem() {
...
this->DeclareAbstractOutputPort(
&AllocInConstructorSystem::CalcFramePoseOutput);
...
}

private:
void CalcFramePoseOutput(const Context<T>& context,
geometry::FramePoseVector<T>* poses) const {
poses->clear();
for (int i = 0; i < static_cast<int>(frame_ids_.size()); ++i) {
poses->set_value(frame_ids_[i], poses_[i]);
}
}

std::vector<FrameId> frame_ids_;
std::vector<RigidTransform<T>> poses_;
};

If a System only ever emits a single frame (or small-constant-number of frames), then there’s a shorter alternative way to write a Calc method, using an initializer_list:

void CalcFramePoseOutput(const Context<T>& context,
geometry::FramePoseVector<T>* poses) const {
const RigidTransform<T>& pose = ...;
poses = {{frame_id_, pose}};
}

N.B. When the systems framework calls the Calc method, the value pointed to by poses is in an unspecified state. The implementation of Calc must always ensure that poses contains the correct value upon return, no matter what value it started with. The easy ways to do this are to call either poses->clear() or the assignment operator *poses = ....

Template parameter KinematicsValue:
The underlying data type of for the order of kinematics data (e.g., pose, velocity, or acceleration).

One should never interact with the FrameKinematicsVector class directly. Instead, the FramePoseVector, FrameVelocityVector, and FrameAccelerationVector classes are aliases of the FrameKinematicsVector instantiated on specific data types (RigidTransform, SpatialVector, and SpatialAcceleration, respectively). Each of these data types are templated on Eigen scalars. All supported combinations of data type and scalar type are already available to link against in the containing library. No other values for KinematicsValue are supported.

Currently, the following data types with the following scalar types are supported:

Alias | Instantiation | Scalar types —————–|-----------------------------------------------|————- FramePoseVector | FrameKinematicsVector<RigidTransform<Scalar>> | double FramePoseVector | FrameKinematicsVector<RigidTransform<Scalar>> | AutoDiffXd FramePoseVector | FrameKinematicsVector<RigidTransform<Scalar>> | Expression

__init__(self: pydrake.geometry.FramePoseVector_[float]) → None

Initializes the vector using an invalid SourceId with no frames .

clear(self: pydrake.geometry.FramePoseVector_[float]) → None

Clears all values, resetting the size to zero.

frame_ids(self: pydrake.geometry.FramePoseVector_[float]) → List[pydrake.geometry.FrameId]

Provides a range object for all of the frame ids in the vector. This is intended to be used as:

for (FrameId id : this_vector.frame_ids()) {
...
// Obtain the KinematicsValue of an id by ``this_vector.value(id)``
...
}
has_id(self: pydrake.geometry.FramePoseVector_[float], id: pydrake.geometry.FrameId) → bool

Reports true if the given id is a member of this data.

set_value(self: pydrake.geometry.FramePoseVector_[float], id: pydrake.geometry.FrameId, value: pydrake.math.RigidTransform_[float]) → None

Sets the kinematics value for the frame indicated by the given id.

size(self: pydrake.geometry.FramePoseVector_[float]) → int

Returns number of frame_ids().

value(self: pydrake.geometry.FramePoseVector_[float], id: pydrake.geometry.FrameId) → pydrake.math.RigidTransform_[float]

Returns the value associated with the given id.

Raises:RuntimeError if id is not in the specified set of ids.
class pydrake.geometry.FramePoseVector_[AutoDiffXd]

A FrameKinematicsVector is used to report kinematics data for registered frames (identified by unique FrameId values) to SceneGraph. It serves as the basis of FramePoseVector, FrameVelocityVector, and FrameAccelerationVector.

template <typename T>
class MySystem : public LeafSystem<T> {
public:
MySystem() {
...
this->DeclareAbstractOutputPort(
&AllocInConstructorSystem::CalcFramePoseOutput);
...
}

private:
void CalcFramePoseOutput(const Context<T>& context,
geometry::FramePoseVector<T>* poses) const {
poses->clear();
for (int i = 0; i < static_cast<int>(frame_ids_.size()); ++i) {
poses->set_value(frame_ids_[i], poses_[i]);
}
}

std::vector<FrameId> frame_ids_;
std::vector<RigidTransform<T>> poses_;
};

If a System only ever emits a single frame (or small-constant-number of frames), then there’s a shorter alternative way to write a Calc method, using an initializer_list:

void CalcFramePoseOutput(const Context<T>& context,
geometry::FramePoseVector<T>* poses) const {
const RigidTransform<T>& pose = ...;
poses = {{frame_id_, pose}};
}

N.B. When the systems framework calls the Calc method, the value pointed to by poses is in an unspecified state. The implementation of Calc must always ensure that poses contains the correct value upon return, no matter what value it started with. The easy ways to do this are to call either poses->clear() or the assignment operator *poses = ....

Template parameter KinematicsValue:
The underlying data type of for the order of kinematics data (e.g., pose, velocity, or acceleration).

One should never interact with the FrameKinematicsVector class directly. Instead, the FramePoseVector, FrameVelocityVector, and FrameAccelerationVector classes are aliases of the FrameKinematicsVector instantiated on specific data types (RigidTransform, SpatialVector, and SpatialAcceleration, respectively). Each of these data types are templated on Eigen scalars. All supported combinations of data type and scalar type are already available to link against in the containing library. No other values for KinematicsValue are supported.

Currently, the following data types with the following scalar types are supported:

Alias | Instantiation | Scalar types —————–|-----------------------------------------------|————- FramePoseVector | FrameKinematicsVector<RigidTransform<Scalar>> | double FramePoseVector | FrameKinematicsVector<RigidTransform<Scalar>> | AutoDiffXd FramePoseVector | FrameKinematicsVector<RigidTransform<Scalar>> | Expression

__init__(self: pydrake.geometry.FramePoseVector_[AutoDiffXd]) → None

Initializes the vector using an invalid SourceId with no frames .

clear(self: pydrake.geometry.FramePoseVector_[AutoDiffXd]) → None

Clears all values, resetting the size to zero.

frame_ids(self: pydrake.geometry.FramePoseVector_[AutoDiffXd]) → List[pydrake.geometry.FrameId]

Provides a range object for all of the frame ids in the vector. This is intended to be used as:

for (FrameId id : this_vector.frame_ids()) {
...
// Obtain the KinematicsValue of an id by ``this_vector.value(id)``
...
}
has_id(self: pydrake.geometry.FramePoseVector_[AutoDiffXd], id: pydrake.geometry.FrameId) → bool

Reports true if the given id is a member of this data.

set_value(self: pydrake.geometry.FramePoseVector_[AutoDiffXd], id: pydrake.geometry.FrameId, value: pydrake.math.RigidTransform_[AutoDiffXd]) → None

Sets the kinematics value for the frame indicated by the given id.

size(self: pydrake.geometry.FramePoseVector_[AutoDiffXd]) → int

Returns number of frame_ids().

value(self: pydrake.geometry.FramePoseVector_[AutoDiffXd], id: pydrake.geometry.FrameId) → pydrake.math.RigidTransform_[AutoDiffXd]

Returns the value associated with the given id.

Raises:RuntimeError if id is not in the specified set of ids.
class pydrake.geometry.FramePoseVector_[float]

A FrameKinematicsVector is used to report kinematics data for registered frames (identified by unique FrameId values) to SceneGraph. It serves as the basis of FramePoseVector, FrameVelocityVector, and FrameAccelerationVector.

template <typename T>
class MySystem : public LeafSystem<T> {
public:
MySystem() {
...
this->DeclareAbstractOutputPort(
&AllocInConstructorSystem::CalcFramePoseOutput);
...
}

private:
void CalcFramePoseOutput(const Context<T>& context,
geometry::FramePoseVector<T>* poses) const {
poses->clear();
for (int i = 0; i < static_cast<int>(frame_ids_.size()); ++i) {
poses->set_value(frame_ids_[i], poses_[i]);
}
}

std::vector<FrameId> frame_ids_;
std::vector<RigidTransform<T>> poses_;
};

If a System only ever emits a single frame (or small-constant-number of frames), then there’s a shorter alternative way to write a Calc method, using an initializer_list:

void CalcFramePoseOutput(const Context<T>& context,
geometry::FramePoseVector<T>* poses) const {
const RigidTransform<T>& pose = ...;
poses = {{frame_id_, pose}};
}

N.B. When the systems framework calls the Calc method, the value pointed to by poses is in an unspecified state. The implementation of Calc must always ensure that poses contains the correct value upon return, no matter what value it started with. The easy ways to do this are to call either poses->clear() or the assignment operator *poses = ....

Template parameter KinematicsValue:
The underlying data type of for the order of kinematics data (e.g., pose, velocity, or acceleration).

One should never interact with the FrameKinematicsVector class directly. Instead, the FramePoseVector, FrameVelocityVector, and FrameAccelerationVector classes are aliases of the FrameKinematicsVector instantiated on specific data types (RigidTransform, SpatialVector, and SpatialAcceleration, respectively). Each of these data types are templated on Eigen scalars. All supported combinations of data type and scalar type are already available to link against in the containing library. No other values for KinematicsValue are supported.

Currently, the following data types with the following scalar types are supported:

Alias | Instantiation | Scalar types —————–|-----------------------------------------------|————- FramePoseVector | FrameKinematicsVector<RigidTransform<Scalar>> | double FramePoseVector | FrameKinematicsVector<RigidTransform<Scalar>> | AutoDiffXd FramePoseVector | FrameKinematicsVector<RigidTransform<Scalar>> | Expression

__init__(self: pydrake.geometry.FramePoseVector_[float]) → None

Initializes the vector using an invalid SourceId with no frames .

clear(self: pydrake.geometry.FramePoseVector_[float]) → None

Clears all values, resetting the size to zero.

frame_ids(self: pydrake.geometry.FramePoseVector_[float]) → List[pydrake.geometry.FrameId]

Provides a range object for all of the frame ids in the vector. This is intended to be used as:

for (FrameId id : this_vector.frame_ids()) {
...
// Obtain the KinematicsValue of an id by ``this_vector.value(id)``
...
}
has_id(self: pydrake.geometry.FramePoseVector_[float], id: pydrake.geometry.FrameId) → bool

Reports true if the given id is a member of this data.

set_value(self: pydrake.geometry.FramePoseVector_[float], id: pydrake.geometry.FrameId, value: pydrake.math.RigidTransform_[float]) → None

Sets the kinematics value for the frame indicated by the given id.

size(self: pydrake.geometry.FramePoseVector_[float]) → int

Returns number of frame_ids().

value(self: pydrake.geometry.FramePoseVector_[float], id: pydrake.geometry.FrameId) → pydrake.math.RigidTransform_[float]

Returns the value associated with the given id.

Raises:RuntimeError if id is not in the specified set of ids.
class pydrake.geometry.GeometryFrame

This simple class carries the definition of a frame used in the SceneGraph. To register moving frames with SceneGraph (see SceneGraph::RegisterFrame()), a geometry source (see SceneGraph::RegisterSource()) instantiates a frame and passes ownership over to SceneGraph.

A frame is defined by two pieces of information:

  • the name, which must be unique within a single geometry source and
  • the “frame group”, an integer identifier that can be used to group frames

together within a geometry source.

The “frame group” is intended as a generic synonym for the model instance id defined by the RigidBodyTree.

See also

SceneGraph

__init__(self: pydrake.geometry.GeometryFrame, frame_name: str, frame_group_id: int = 0) → None

Constructor.

Parameter frame_name:
The name of the frame.
Parameter frame_group_id:
The optional frame group identifier. If unspecified, defaults to the common, 0 group. Must be non-negative.
frame_group(self: pydrake.geometry.GeometryFrame) → int
id(self: pydrake.geometry.GeometryFrame) → pydrake.geometry.FrameId

Returns the globally unique id for this geometry specification. Every instantiation of FrameInstance will contain a unique id value. The id value is preserved across copies. After successfully registering this FrameInstance, this id will serve as the identifier for the registered representation as well.

name(self: pydrake.geometry.GeometryFrame) → str
class pydrake.geometry.GeometryId

Type used to identify geometry instances in SceneGraph.

__init__

Initialize self. See help(type(self)) for accurate signature.

static get_new_id() → pydrake.geometry.GeometryId

Generates a new identifier for this id type. This new identifier will be different from all previous identifiers created. This method does not make any guarantees about the values of ids from successive invocations. This method is guaranteed to be thread safe.

get_value(self: pydrake.geometry.GeometryId) → int

Extracts the underlying representation from the identifier. This is considered invalid for invalid ids and is strictly enforced in Debug builds.

is_valid(self: pydrake.geometry.GeometryId) → bool

Reports if the id is valid.

class pydrake.geometry.GeometryInstance

A geometry instance combines a geometry definition (i.e., a shape of some sort), a pose (relative to a parent “frame” P), material information, and an opaque collection of metadata. The parent frame can be a registered frame or another registered geometry.

Every GeometryInstance must be named. The naming convention mirrors that of valid names in SDF files. Specifically, any user-specified name will have all leading and trailing space and tab characters trimmed off. The trimmed name will have to satisfy the following requirements:

  • cannot be empty, and
  • the name should be unique in the scope of its frame and role. For example,

two GeometryInstances can both be called “ball” as long as they are affixed to different frames or if one is a collision geometry and the other is an illustration geometry. This is enforced when a role is assigned to the geometry.

If valid, the trimmed name will be assigned to the instance.

Names can have internal whitespace (e.g., “my geometry name”).

Canonicalized names

The silent transformation of a user-defined name to canonical name mirrors that of specifying geometry names in an SDF file. Consider the following SDF snippet:

{xml}
...
<visual name="  visual">
<geometry>
<sphere>
<radius>1.0</radius>
</sphere>
</geometry>
</visual>
...

The name has two leading whitespace characters. The parsing process will consider this name as equivalent to “visual” and tests for uniqueness and non-emptiness will be applied to that trimmed result. The following code has an analogous effect:

scene_graph->RegisterGeometry(
source_id, frame_id,
make_unique<GeometryInstance>(pose, make_unique<Sphere>(1.0), "  visual"));

The specified name includes leading whitespace. That name will be trimmed and the result will be stored in the GeometryInstance (to be later validated by SceneGraph as part of geometry registration). Querying the instance of its name will return this canonicalized name.

__init__(self: pydrake.geometry.GeometryInstance, X_PG: pydrake.math.RigidTransform_[float], shape: pydrake.geometry.Shape, name: str) → None

Constructs a geometry instance specification.

Parameter X_PG:
The pose of this geometry (G) in its parent’s frame (P).
Parameter shape:
The underlying shape for this geometry instance.
Parameter name:
The name of the geometry (must satisfy the name requirements).
Raises:RuntimeError if the canonicalized version of name is empty.
id(self: pydrake.geometry.GeometryInstance) → pydrake.geometry.GeometryId

Returns the globally unique id for this geometry specification. Every instantiation of GeometryInstance will contain a unique id value. The id value is preserved across copies. After successfully registering this GeometryInstance, this id will serve as the identifier for the registered representation as well.

illustration_properties(self: pydrake.geometry.GeometryInstance) → drake::geometry::IllustrationProperties

Returns a pointer to the geometry’s const illustration properties (if they are defined). Nullptr otherwise.

mutable_illustration_properties(self: pydrake.geometry.GeometryInstance) → drake::geometry::IllustrationProperties

Returns a pointer to the geometry’s mutable illustration properties (if they are defined). Nullptr otherwise.

mutable_perception_properties(self: pydrake.geometry.GeometryInstance) → drake::geometry::PerceptionProperties

Returns a pointer to the geometry’s mutable perception properties (if they are defined). Nullptr otherwise.

mutable_proximity_properties(self: pydrake.geometry.GeometryInstance) → drake::geometry::ProximityProperties

Returns a pointer to the geometry’s mutable proximity properties (if they are defined). Nullptr otherwise.

name(self: pydrake.geometry.GeometryInstance) → str

Returns the canonicalized name for the instance.

See also

canonicalized_geometry_names “Canonicalized names”

perception_properties(self: pydrake.geometry.GeometryInstance) → drake::geometry::PerceptionProperties

Returns a pointer to the geometry’s const perception properties (if they are defined). Nullptr otherwise.

pose(self: pydrake.geometry.GeometryInstance) → pydrake.math.RigidTransform_[float]

Returns the instance geometry’s pose in its parent frame.

proximity_properties(self: pydrake.geometry.GeometryInstance) → drake::geometry::ProximityProperties

Returns a pointer to the geometry’s const proximity properties (if they are defined). Nullptr otherwise.

release_shape(self: pydrake.geometry.GeometryInstance) → pydrake.geometry.Shape

Releases the shape from the instance.

set_illustration_properties(self: pydrake.geometry.GeometryInstance, properties: drake::geometry::IllustrationProperties) → None

Sets the illustration properties for the given instance.

set_name(self: pydrake.geometry.GeometryInstance, arg0: str) → None

Sets the canonicalized name for the instance.

See also

canonicalized_geometry_names “Canonicalized names”

set_perception_properties(self: pydrake.geometry.GeometryInstance, properties: drake::geometry::PerceptionProperties) → None

Sets the perception properties for the given instance.

set_pose(self: pydrake.geometry.GeometryInstance, X_PG: pydrake.math.RigidTransform_[float]) → None

Sets the pose of this instance in its parent’s frame.

set_proximity_properties(self: pydrake.geometry.GeometryInstance, properties: drake::geometry::ProximityProperties) → None

Sets the proximity properties for the given instance.

shape(self: pydrake.geometry.GeometryInstance) → pydrake.geometry.Shape
class pydrake.geometry.GeometryProperties

The base class for defining a set of geometry properties.

Each property consists of a (group, property) name-pair and a typed value. The name pair allows for reuse of common property names (e.g., “diffuse”) to be differentiated in interpretation by associating them with different groups. The only restriction on the value type is that it must be either cloneable or copy-constructible.

A set of geometry property values are defined when geometry is registered with SceneGraph by an instantiator and accessed by some downstream consumer entity. Each consumer specifies what properties it expects to find and what default values (if any) it provides. For example, the consumer could document that a particular property is always required and its absence would throw an exception. Alternatively, it could indicate that a property is optional and a default value will be used in its absence. It is the responsibility of the instantiator to make sure that the geometry property values are correctly defined according to the expected consumer’s specification. Correctness includes such issues as key-value pairs placed into a correctly-spelled group, property keys being likewise correctly spelled, and values of the expected type. Correct spelling includes correct case. The instantiator uses the AddProperty() method to add new properties to the set.

To read the property (some_group, some_property) from a property set:

1. Optionally test to see if the property exists by confirming the group some_group is in the set via HasGroup() and that the property some_property is in some_group via HasProperty(). Attempting to access a property with a non-existent (group, property) pair may lead to an exception (see API documentation below). 2. Acquire a property value via the GetProperty() or GetPropertyOrDefault() methods. NOTE: Reading a property requires a compile-time declaration of the type of value being read. If the stored value is of a different type, an exception will be thrown.

The following examples outline a number of ways to create and consume geometry properties. By design, GeometryProperties cannot be constructed, copied, or moved directly. Only derived classes can do so. This facilitates strongly typed sets of properties associated with particular geometry roles. So, for these examples we’ll exercise the derived class associated with proximity queries: ProximityProperties.

The string-based structure of GeometryProperties provides a great deal of flexibility at the cost of spelling sensitivity. It would be easy to introduce typos that would then “hide” property values in some group a consumer wouldn’t look. In these examples, we avoid using string literals as group or property names (at least in the cases where the same name is used multiple times) to help avoid the possibility of typo-induced errors. That is not required and certainly not the only way to avoid such bugs.

Creating properties

Creating properties in a new group

This is a simple example in which a single group is added with properties of various types.

const std::string group_name("my_group");
ProximityProperties properties;
// This first invocation implicitly creates the group "my_group".
properties.AddProperty(group_name, "count", 7);     // int type
properties.AddProperty(group_name, "length", 7.);   // double type
properties.AddProperty(group_name, "name", "7");    // std::string type

Creating properties in the default group

Similar to the previous examples, the properties are added to the default group. Just be aware that if multiple sites in your code add properties to the default group, the possibility that names get repeated increases. Property names must be unique within a single group, including the default group.

ProximityProperties properties;
properties.AddProperty(ProximityProperties::default_group_name(), "count", 7);
properties.AddProperty(ProximityProperties::default_group_name(), "width", 7.);
properties.AddProperty(ProximityProperties::default_group_name(), "name", "7");

Aggregate properties in a struct

In some cases, there is a set of values that will always be accessed together (specified with coordinated semantics). In these cases, it makes sense to aggregate them into a struct and store that as a single value. This reduces the number of lookups required.

It’s worth noting, that if the data value is a struct, calls to GetPropertyOrDefault() still operate as an “all-or-nothing” basis. If the property struct exists, it will be returned, if it’s missing the default struct will be returned. There is no concept of a “partial” struct in which some undefined values in the struct will be replaced with their corresponding values in the default struct.

struct MyData {
int i{};
double d{};
std::string s;
};

ProximityProperties properties;
const std::string group_name("my_group");
MyData data{7, 7., "7"};
properties.AddProperty(group_name, "data1", data);
// These alternate forms are also acceptable (but not in succession, as the
// property name has already been used by the first invocation).
properties.AddProperty(group_name, "data2", MyData{6, 6., "6"});
properties.AddProperty<MyData>(group_name, "data2", {6, 6., "6"});

Reading properties

This section describes how to read properties under several different scenarios: (a) when specific properties are required, (b) when the consumer provides a default value for missing properties, and (c) when the consumer needs to inspect what properties are available.

Look up specific, *required properties*

In this case, the consumer of the properties is looking for one or more specific properties. It will ignore any other properties. More particularly, if those properties are missing, it is considered a runtime error and an exception is thrown.

The error can be handled in one of two ways: simply let the generic exception generated by GeometryProperties propagate upward, or detect the missing property and throw an exception with a custom message. The example below shows both approaches.

const IllustrationProperties& properties = FunctionThatReturnsProperties();
// Looking for a Rgba of rgba colors named "rgba" - send generic error that
// the property set is missing the required property.
const Rgba rgba =
properties.GetProperty<Rgba>("MyGroup", "rgba");

// Explicitly detect missing property and throw exception with custom message.
if (!properties.HasProperty("MyGroup", "rgba")) {
throw RuntimeError(
"ThisClass: Missing the necessary 'rgba' property; the object cannot be "
"rendered");
}
// Otherwise acquire value, confident that no exception will be thrown.
const Rgba rgba =
properties.GetProperty<Rgba>("MyGroup", "rgba");

Note

calls to GetProperty() always require the return type template value (e.g., Rgba) to be specified in the call.

Look up specific properties with default property values

As with the previous case, the consumer is looking for one or more specific properties. However, in this case, the consumer provides a default value to use in case the target property is not defined. In this invocation, the template parameter need not be explicitly declared – the inferred return type will be the same as the default value.

const IllustrationProperties& properties = FunctionThatReturnsProperties();
// Looking for a Rgba of rgba colors named "rgba".
const Rgba default_color{0.9, 0.9, 0.9};
const Rgba rgba =
properties.GetPropertyOrDefault("MyGroup", "rgba", default_color);

Alternatively, the default value can be provided in one of the following forms:

properties.GetPropertyOrDefault("MyGroup", "rgba",
Rgba{0.9, 0.9, 0.9});
properties.GetPropertyOrDefault<Rgba>("MyGroup", "rgba",
{0.9, 0.9, 0.9});

Iterating through provided properties

Another alternative is to iterate through the properties that have been provided. This might be done for several reasons, e.g.:

  • the consumer wants to validate the set of properties, giving the user

feedback if an unsupported property has been provided, and/or - the consumer has a default value for every property and allows the registering code to define only those properties that deviate from the specified default.

Working with properties in this manner requires knowledge of how to work with AbstractValue.

const IllustrationProperties& properties = FunctionThatReturnsProperties();
for (const auto& pair : properties.GetGroupProperties("MyGroup") {
const std::string& name = pair.first;
if (name == "rgba") {
// Throws an exception if the named parameter is of the wrong type.
const Rgba& rgba =
pair.second->GetValueOrThrow<Rgba>();
}
}
__init__

Initialize self. See help(type(self)) for accurate signature.

AddProperty(self: pydrake.geometry.GeometryProperties, group_name: str, name: str, value: object) → None

Adds the named property (group_name, name) with the given value. Adds the group if it doesn’t already exist.

Parameter group_name:
The group name.
Parameter name:
The name of the property – must be unique in the group.
Parameter value:
The value to assign to the property.
Raises:RuntimeError if the property already exists.
Template parameter ValueType:
The type of data to store with the attribute – must be copy constructible or cloneable (see Value).
static default_group_name() → str

Returns the default group name. There is no guarantee as to what string corresponds to the default group. Therefore it should always be accessed via this method.

GetGroupNames(self: pydrake.geometry.GeometryProperties) → Set[str]

Returns all of the defined group names.

GetPropertiesInGroup(self: pydrake.geometry.GeometryProperties, group_name: str) → dict

Retrieves the indicated property group. The returned group is valid for as long as this instance.

Raises:RuntimeError if there is no group with the given name.
GetProperty(self: pydrake.geometry.GeometryProperties, group_name: str, name: str) → object

Retrieves the typed value for the property (group_name, name) from this set of properties.

Parameter group_name:
The name of the group to which the property belongs.
Parameter name:
The name of the desired property.
Raises:
  • RuntimeError if a) the group name is invalid, b) the property name
  • is invalid, or c) the property type is not that specified.
Template parameter ValueType:
The expected type of the desired property.
Returns:const ValueType& of stored value. If ValueType is Eigen::Vector4d, the return type will be a copy translated from Rgba.
GetPropertyOrDefault(self: pydrake.geometry.GeometryProperties, group_name: str, name: str, default_value: object) → object

Retrieves the typed value for the property (group_name, name) from the set of properties (if it exists), otherwise returns the given default value. The given default_value is returned only if the property is missing. If the property exists and is of a different type, an exception will be thrown. If it is of the expected type, the stored value will be returned.

Generally, it is unnecessary to explicitly declare the ValueType of the property value; it will be inferred from the provided default value. Sometimes it is convenient to provide the default value in a form that can be implicitly converted to the final type. In that case, it is necessary to explicitly declare the desired ValueType so the compiler does not infer the wrong type, e.g.:

// Note the *integer* value as default value.
const double my_value = properties.GetPropertyOrDefault<double>("g", "p", 2);
Parameter group_name:
The name of the group to which the property belongs.
Parameter name:
The name of the desired property.
Parameter default_value:
The alternate value to return if the property cannot be acquired.
Raises:
  • RuntimeError if a property of the given name exists but is not of
  • ValueType.
HasGroup(self: pydrake.geometry.GeometryProperties, group_name: str) → bool

Reports if the given named group is part of this property set.

HasProperty(self: pydrake.geometry.GeometryProperties, group_name: str, name: str) → bool

Reports if the property (group_name, name) exists in the group.

Parameter group_name:
The name of the group to which the tested property should belong.
Parameter name:
The name of the property under question.
Returns:true iff the group exists and a property with the given name exists in that group.
num_groups(self: pydrake.geometry.GeometryProperties) → int

Reports the number of property groups in this set.

RemoveProperty(self: pydrake.geometry.GeometryProperties, group_name: str, name: str) → bool

Removes the (group_name, name) property (if it exists). Upon completion the property will not be in the set.

Returns:True if the property existed prior to the call.
UpdateProperty(self: pydrake.geometry.GeometryProperties, group_name: str, name: str, value: object) → None

Updates the named property (group_name, name) with the given value. If the property doesn’t already exist, it is equivalent to calling AddProperty. If the property does exist, its value (which must have the same type as value) will be replaced.

Parameter group_name:
The group name.
Parameter name:
The name of the property – must be unique in the group.
Parameter value:
The value to assign to the property.
Raises:RuntimeError if the property exists with a different type.
Template parameter ValueType:
The type of data to store with the attribute – must be copy constructible or cloneable (see Value).
class pydrake.geometry.GeometrySet

The GeometrySet, as its name implies, is a convenience class for defining a set of geometries. What makes it unique from a simple std::set<GeometryId> instance is that membership doesn’t require explicit GeometryId enumeration; GeometryId values can be added to the set by adding the FrameId for the frame to which the geometries are rigidly affixed.

This class does no validation; it is a simple collection. Ultimately, it serves as the operand of SceneGraph operations (e.g., SceneGraph::ExcludeCollisionsWithin()). If the operation has a particular prerequisite on the members of a GeometrySet, it is the operation’s responsibility to enforce that requirement.

More formally, the SceneGraph consists of a set of geometries, each associated with a unique identifier. As such, we can consider the set of all identifiers SG = {g₀, g₁, ..., gₙ} that belong to a SceneGraph. A GeometrySet should represent a subset of those identifiers, Gₛ SG. The convenience of the GeometrySet class is how the subset is defined. Given a set of frame ids F = {f₀, f₁, ..., fₙ} and geometry ids G = {g₀, g₁, ..., gₘ}, Gₛ = G ⋃ geometry(f₀) ⋃ … ⋃ geometry(fₙ) (where geometry(f) is the set of geometries rigidly affixed to frame f).

__init__(self: pydrake.geometry.GeometrySet) → None
class pydrake.geometry.HalfSpace

Bases: pydrake.geometry.Shape

Definition of a half space. In its canonical frame, the plane defining the boundary of the half space is that frame’s z = 0 plane. By implication, the plane’s normal points in the +z direction and the origin lies on the plane. Other shapes are considered to be penetrating the half space if there exists a point on the test shape that lies on the side of the plane opposite the normal.

__init__(self: pydrake.geometry.HalfSpace) → None
static MakePose(Hz_dir_F: numpy.ndarray[numpy.float64[3, 1]], p_FB: numpy.ndarray[numpy.float64[3, 1]]) → pydrake.math.RigidTransform_[float]

Creates the pose of a canonical half space in frame F. The half space’s normal is aligned to the positive z-axis of its canonical frame H. Given a vector that points in the same direction, measured in the F frame (Hz_dir_F) and a position vector to a point on the half space’s boundary* expressed in the same frame, p_FB, creates the pose of the half space in frame F: X_FH.

Parameter Hz_dir_F:
A vector in the direction of the positive z-axis of the canonical frame expressed in frame F. It must be a non-zero vector but need not be unit length.
Parameter p_FB:
A point B lying on the half space’s boundary measured and expressed in frame F.
Returns X_FH:
The pose of the canonical half-space in frame F.
Raises:
  • RuntimeError if the normal is close to a zero-vector (e.g.,
  • ‖normal_F‖₂ < ε).
class pydrake.geometry.IllustrationProperties

Bases: pydrake.geometry.GeometryProperties

The set of properties for geometry used in an “illustration” role.

Examples of functionality that depends on the illustration role: - geometry_visualization_role_dependency “drake::geometry::ConnectDrakeVisualizer()”

__init__(self: pydrake.geometry.IllustrationProperties) → None
pydrake.geometry.MakePhongIllustrationProperties(diffuse: numpy.ndarray[numpy.float64[4, 1]]) → pydrake.geometry.IllustrationProperties

Constructs an IllustrationProperties instance compatible with a simple “phong” material using only the given diffuse color.

class pydrake.geometry.Mesh

Bases: pydrake.geometry.Shape

Limited support for meshes. Meshes are supported in Rendering and Illustration roles. For Proximity role, Meshes are supported in ComputeContactSurfaces() query only. No other proximity queries are supported.

__init__(self: pydrake.geometry.Mesh, absolute_filename: str, scale: float = 1.0) → None

Constructs a mesh specification from the mesh file located at the given absolute file path. Optionally uniformly scaled by the given scale factor.

Raises:
  • RuntimeError if |scale| < 1e-8. Note that a negative scale is
  • considered valid. We want to preclude scales near zero but
  • recognise that scale is a convenience tool for “tweaking” models.
  • 8 orders of magnitude should be plenty without considering
  • revisiting the model itself.
filename(self: pydrake.geometry.Mesh) → str
scale(self: pydrake.geometry.Mesh) → float
pydrake.geometry.PenetrationAsPointPair

alias of pydrake.geometry.PenetrationAsPointPair_[float]

template pydrake.geometry.PenetrationAsPointPair_

Instantiations: PenetrationAsPointPair_[float], PenetrationAsPointPair_[AutoDiffXd]

class PenetrationAsPointPair_[float]

A characterization of the intersection of two penetrating geometries. The characterization consists of a pair of points and a normal. The points represent a point on each geometry that most deeply penetrates the other geometry (in the normal direction). For convenience, the penetration depth is provided and is equal to:

depth = (p_WCb - p_WCa) nhat_BA_W

(depth is strictly positive when there is penetration and otherwise not defined.)

Template parameter T:
The underlying scalar type. Must be a valid Eigen scalar.
__init__(self: pydrake.geometry.PenetrationAsPointPair_[float], **kwargs) → None
depth

The penetration depth.

id_A

The id of the first geometry in the contact.

id_B

The id of the second geometry in the contact.

nhat_BA_W

The unit-length normal which defines the penetration direction, pointing from geometry B into geometry A, measured and expressed in the world frame. It approximates the normal to the plane on which the contact patch lies.

p_WCa

The point on A that most deeply penetrates B, measured and expressed in the world frame.

p_WCb

The point on B that most deeply penetrates A, measured and expressed in the world frame.

class pydrake.geometry.PenetrationAsPointPair_[AutoDiffXd]

A characterization of the intersection of two penetrating geometries. The characterization consists of a pair of points and a normal. The points represent a point on each geometry that most deeply penetrates the other geometry (in the normal direction). For convenience, the penetration depth is provided and is equal to:

depth = (p_WCb - p_WCa) nhat_BA_W

(depth is strictly positive when there is penetration and otherwise not defined.)

Template parameter T:
The underlying scalar type. Must be a valid Eigen scalar.
__init__(self: pydrake.geometry.PenetrationAsPointPair_[AutoDiffXd], **kwargs) → None
depth

The penetration depth.

id_A

The id of the first geometry in the contact.

id_B

The id of the second geometry in the contact.

nhat_BA_W

The unit-length normal which defines the penetration direction, pointing from geometry B into geometry A, measured and expressed in the world frame. It approximates the normal to the plane on which the contact patch lies.

p_WCa

The point on A that most deeply penetrates B, measured and expressed in the world frame.

p_WCb

The point on B that most deeply penetrates A, measured and expressed in the world frame.

class pydrake.geometry.PenetrationAsPointPair_[float]

A characterization of the intersection of two penetrating geometries. The characterization consists of a pair of points and a normal. The points represent a point on each geometry that most deeply penetrates the other geometry (in the normal direction). For convenience, the penetration depth is provided and is equal to:

depth = (p_WCb - p_WCa) nhat_BA_W

(depth is strictly positive when there is penetration and otherwise not defined.)

Template parameter T:
The underlying scalar type. Must be a valid Eigen scalar.
__init__(self: pydrake.geometry.PenetrationAsPointPair_[float], **kwargs) → None
depth

The penetration depth.

id_A

The id of the first geometry in the contact.

id_B

The id of the second geometry in the contact.

nhat_BA_W

The unit-length normal which defines the penetration direction, pointing from geometry B into geometry A, measured and expressed in the world frame. It approximates the normal to the plane on which the contact patch lies.

p_WCa

The point on A that most deeply penetrates B, measured and expressed in the world frame.

p_WCb

The point on B that most deeply penetrates A, measured and expressed in the world frame.

class pydrake.geometry.PerceptionProperties

Bases: pydrake.geometry.GeometryProperties

The set of properties for geometry used in a “perception” role.

Examples of functionality that depends on the perception role: - render::RenderEngineVtk - render::RenderEngineOspray

__init__(self: pydrake.geometry.PerceptionProperties) → None
class pydrake.geometry.ProximityProperties

Bases: pydrake.geometry.GeometryProperties

The set of properties for geometry used in a proximity role.

Examples of functionality that depends on the proximity role:

__init__(self: pydrake.geometry.ProximityProperties) → None
pydrake.geometry.QueryObject

alias of pydrake.geometry.QueryObject_[float]

template pydrake.geometry.QueryObject_

Instantiations: QueryObject_[float], QueryObject_[AutoDiffXd]

class QueryObject_[float]

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.

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. In some cases the level of support is obvious (such as when the query is declared explicitly in terms of a double-valued scalar – see ComputePointPairPenetration()). In other cases, where the query is expressed in terms of scalar T, the query may have restrictions. If a query has restricted scalar support, it is included in the query’s documentation.

__init__(self: pydrake.geometry.QueryObject_[float]) → None

Constructs a default QueryObject (all pointers are null).

ComputePointPairPenetration(self: pydrake.geometry.QueryObject_[float]) → List[drake::geometry::PenetrationAsPointPair<double>]

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.

Scalar support

This method only provides double-valued penetration results.

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

This silently ignores Mesh geometries (but Convex mesh geometries are included).

ComputeSignedDistancePairClosestPoints(self: pydrake.geometry.QueryObject_[float], id_A: pydrake.geometry.GeometryId, id_B: pydrake.geometry.GeometryId) → drake::geometry::SignedDistancePair<double>

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().

Raises:
  • if either geometry id is invalid, or if the pair (id_A, id_B) has
  • been marked as filtered.
ComputeSignedDistancePairwiseClosestPoints(self: pydrake.geometry.QueryObject_[float], max_distance: float = inf) → List[drake::geometry::SignedDistancePair<double>]

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.

Scalar support

This function does not support halfspaces. If an unfiltered pair contains a halfspace, an exception will be thrown for all scalar types. Otherwise, this query supports all other pairs of Drake geometry types for double. For AutoDiffXd, it only supports distance between sphere-box and sphere-sphere. If there are any unfiltered geometry pairs that include other geometries, the AutoDiff throws an exception.

Parameter 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.
ComputeSignedDistanceToPoint(self: pydrake.geometry.QueryObject_[float], p_WQ: numpy.ndarray[numpy.float64[3, 1]], threshold: float = inf) → List[drake::geometry::SignedDistanceToPoint<double>]

Computes the signed distances and gradients to a query point from each geometry in the scene.

Warning

Currently supports spheres, boxes, and cylinders only. Silently ignores other kinds of geometries, which will be added later.

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.

Scalar support

This query only supports computing distances from the point to spheres, boxes, and cylinders for both double and AutoDiffXd scalar types. If the SceneGraph contains any other geometry shapes, they will be silently ignored.

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

Parameter p_WQ:
Position of a query point Q in world frame W.
Parameter threshold:
We ignore any object beyond this distance. By default, it is infinity, so we report distances from the query point to every object.
Returns signed_distances:
A vector populated with per-object signed distance values (and supporting data). See SignedDistanceToPoint.
FindCollisionCandidates(self: pydrake.geometry.QueryObject_[float]) → List[drake::SortedPair<drake::geometry::GeometryId>]

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

Warning

This silently ignores Mesh geometries (but Convex mesh geometries are included).

HasCollisions(self: pydrake.geometry.QueryObject_[float]) → bool

Reports true if there are any collisions between unfiltered pairs in the world.

Warning

This silently ignores Mesh geometries (but Convex mesh geometries are included).

inspector(self: pydrake.geometry.QueryObject_[float]) → pydrake.geometry.SceneGraphInspector_[float]

Provides an inspector for the topological structure of the underlying scene graph data (see SceneGraphInspector for details).

RenderColorImage(self: pydrake.geometry.QueryObject_[float], camera: drake::geometry::render::CameraProperties, parent_frame: pydrake.geometry.FrameId, X_PC: pydrake.math.RigidTransform_[float], show_window: bool = False) → drake::systems::sensors::Image<(drake::systems::sensors::PixelType)2>

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

Parameter camera:
The intrinsic properties of the camera.
Parameter parent_frame:
The id for the camera’s parent frame.
Parameter X_PC:
The pose of the camera body in the world frame.
Parameter show_window:
If true, the render window will be displayed.
Parameter color_image_out:
The rendered color image.
RenderDepthImage(self: pydrake.geometry.QueryObject_[float], camera: drake::geometry::render::DepthCameraProperties, parent_frame: pydrake.geometry.FrameId, X_PC: pydrake.math.RigidTransform_[float]) → drake::systems::sensors::Image<(drake::systems::sensors::PixelType)6>

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.

Parameter camera:
The intrinsic properties of the camera.
Parameter parent_frame:
The id for the camera’s parent frame.
Parameter X_PC:
The pose of the camera body in the world frame.
Parameter depth_image_out:
The rendered depth image.
RenderLabelImage(self: pydrake.geometry.QueryObject_[float], camera: drake::geometry::render::CameraProperties, parent_frame: pydrake.geometry.FrameId, X_PC: pydrake.math.RigidTransform_[float], show_window: bool = False) → drake::systems::sensors::Image<(drake::systems::sensors::PixelType)7>

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

Parameter camera:
The intrinsic properties of the camera.
Parameter parent_frame:
The id for the camera’s parent frame.
Parameter X_PC:
The pose of the camera body in the world frame.
Parameter show_window:
If true, the render window will be displayed.
Parameter label_image_out:
The rendered label image.
X_PF(self: pydrake.geometry.QueryObject_[float], id: pydrake.geometry.FrameId) → pydrake.math.RigidTransform_[float]

Reports the position of the frame indicated by 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 X_WF().

Note

This is analogous to but distinct from SceneGraphInspector::X_PG(). In this case, the pose will always be relative to another frame.

Raises:RuntimeError if the frame id is not valid.
X_WF(self: pydrake.geometry.QueryObject_[float], id: pydrake.geometry.FrameId) → pydrake.math.RigidTransform_[float]

Reports the position of the frame indicated by id relative to the world frame.

Raises:RuntimeError if the frame id is not valid.
X_WG(self: pydrake.geometry.QueryObject_[float], id: pydrake.geometry.GeometryId) → pydrake.math.RigidTransform_[float]

Reports the position of the geometry indicated by id relative to the world frame.

Raises:RuntimeError if the geometry id is not valid.
class pydrake.geometry.QueryObject_[AutoDiffXd]

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.

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. In some cases the level of support is obvious (such as when the query is declared explicitly in terms of a double-valued scalar – see ComputePointPairPenetration()). In other cases, where the query is expressed in terms of scalar T, the query may have restrictions. If a query has restricted scalar support, it is included in the query’s documentation.

__init__(self: pydrake.geometry.QueryObject_[AutoDiffXd]) → None

Constructs a default QueryObject (all pointers are null).

ComputePointPairPenetration(self: pydrake.geometry.QueryObject_[AutoDiffXd]) → List[pydrake.geometry.PenetrationAsPointPair_[float]]

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.

Scalar support

This method only provides double-valued penetration results.

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

This silently ignores Mesh geometries (but Convex mesh geometries are included).

ComputeSignedDistancePairClosestPoints(self: pydrake.geometry.QueryObject_[AutoDiffXd], id_A: pydrake.geometry.GeometryId, id_B: pydrake.geometry.GeometryId) → drake::geometry::SignedDistancePair<Eigen::AutoDiffScalar<Eigen::Matrix<double, -1, 1, 0, -1, 1> > >

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().

Raises:
  • if either geometry id is invalid, or if the pair (id_A, id_B) has
  • been marked as filtered.
ComputeSignedDistancePairwiseClosestPoints(self: pydrake.geometry.QueryObject_[AutoDiffXd], max_distance: float = inf) → List[drake::geometry::SignedDistancePair<Eigen::AutoDiffScalar<Eigen::Matrix<double, -1, 1, 0, -1, 1> > >]

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.

Scalar support

This function does not support halfspaces. If an unfiltered pair contains a halfspace, an exception will be thrown for all scalar types. Otherwise, this query supports all other pairs of Drake geometry types for double. For AutoDiffXd, it only supports distance between sphere-box and sphere-sphere. If there are any unfiltered geometry pairs that include other geometries, the AutoDiff throws an exception.

Parameter 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.
ComputeSignedDistanceToPoint(self: pydrake.geometry.QueryObject_[AutoDiffXd], p_WQ: numpy.ndarray[object[3, 1]], threshold: float = inf) → List[drake::geometry::SignedDistanceToPoint<Eigen::AutoDiffScalar<Eigen::Matrix<double, -1, 1, 0, -1, 1> > >]

Computes the signed distances and gradients to a query point from each geometry in the scene.

Warning

Currently supports spheres, boxes, and cylinders only. Silently ignores other kinds of geometries, which will be added later.

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.

Scalar support

This query only supports computing distances from the point to spheres, boxes, and cylinders for both double and AutoDiffXd scalar types. If the SceneGraph contains any other geometry shapes, they will be silently ignored.

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

Parameter p_WQ:
Position of a query point Q in world frame W.
Parameter threshold:
We ignore any object beyond this distance. By default, it is infinity, so we report distances from the query point to every object.
Returns signed_distances:
A vector populated with per-object signed distance values (and supporting data). See SignedDistanceToPoint.
FindCollisionCandidates(self: pydrake.geometry.QueryObject_[AutoDiffXd]) → List[drake::SortedPair<drake::geometry::GeometryId>]

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

Warning

This silently ignores Mesh geometries (but Convex mesh geometries are included).

HasCollisions(self: pydrake.geometry.QueryObject_[AutoDiffXd]) → bool

Reports true if there are any collisions between unfiltered pairs in the world.

Warning

This silently ignores Mesh geometries (but Convex mesh geometries are included).

inspector(self: pydrake.geometry.QueryObject_[AutoDiffXd]) → pydrake.geometry.SceneGraphInspector_[AutoDiffXd]

Provides an inspector for the topological structure of the underlying scene graph data (see SceneGraphInspector for details).

RenderColorImage(self: pydrake.geometry.QueryObject_[AutoDiffXd], camera: drake::geometry::render::CameraProperties, parent_frame: pydrake.geometry.FrameId, X_PC: pydrake.math.RigidTransform_[float], show_window: bool = False) → drake::systems::sensors::Image<(drake::systems::sensors::PixelType)2>

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

Parameter camera:
The intrinsic properties of the camera.
Parameter parent_frame:
The id for the camera’s parent frame.
Parameter X_PC:
The pose of the camera body in the world frame.
Parameter show_window:
If true, the render window will be displayed.
Parameter color_image_out:
The rendered color image.
RenderDepthImage(self: pydrake.geometry.QueryObject_[AutoDiffXd], camera: drake::geometry::render::DepthCameraProperties, parent_frame: pydrake.geometry.FrameId, X_PC: pydrake.math.RigidTransform_[float]) → drake::systems::sensors::Image<(drake::systems::sensors::PixelType)6>

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.

Parameter camera:
The intrinsic properties of the camera.
Parameter parent_frame:
The id for the camera’s parent frame.
Parameter X_PC:
The pose of the camera body in the world frame.
Parameter depth_image_out:
The rendered depth image.
RenderLabelImage(self: pydrake.geometry.QueryObject_[AutoDiffXd], camera: drake::geometry::render::CameraProperties, parent_frame: pydrake.geometry.FrameId, X_PC: pydrake.math.RigidTransform_[float], show_window: bool = False) → drake::systems::sensors::Image<(drake::systems::sensors::PixelType)7>

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

Parameter camera:
The intrinsic properties of the camera.
Parameter parent_frame:
The id for the camera’s parent frame.
Parameter X_PC:
The pose of the camera body in the world frame.
Parameter show_window:
If true, the render window will be displayed.
Parameter label_image_out:
The rendered label image.
X_PF(self: pydrake.geometry.QueryObject_[AutoDiffXd], id: pydrake.geometry.FrameId) → pydrake.math.RigidTransform_[AutoDiffXd]

Reports the position of the frame indicated by 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 X_WF().

Note

This is analogous to but distinct from SceneGraphInspector::X_PG(). In this case, the pose will always be relative to another frame.

Raises:RuntimeError if the frame id is not valid.
X_WF(self: pydrake.geometry.QueryObject_[AutoDiffXd], id: pydrake.geometry.FrameId) → pydrake.math.RigidTransform_[AutoDiffXd]

Reports the position of the frame indicated by id relative to the world frame.

Raises:RuntimeError if the frame id is not valid.
X_WG(self: pydrake.geometry.QueryObject_[AutoDiffXd], id: pydrake.geometry.GeometryId) → pydrake.math.RigidTransform_[AutoDiffXd]

Reports the position of the geometry indicated by id relative to the world frame.

Raises:RuntimeError if the geometry id is not valid.
class pydrake.geometry.QueryObject_[float]

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.

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. In some cases the level of support is obvious (such as when the query is declared explicitly in terms of a double-valued scalar – see ComputePointPairPenetration()). In other cases, where the query is expressed in terms of scalar T, the query may have restrictions. If a query has restricted scalar support, it is included in the query’s documentation.

__init__(self: pydrake.geometry.QueryObject_[float]) → None

Constructs a default QueryObject (all pointers are null).

ComputePointPairPenetration(self: pydrake.geometry.QueryObject_[float]) → List[drake::geometry::PenetrationAsPointPair<double>]

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.

Scalar support

This method only provides double-valued penetration results.

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

This silently ignores Mesh geometries (but Convex mesh geometries are included).

ComputeSignedDistancePairClosestPoints(self: pydrake.geometry.QueryObject_[float], id_A: pydrake.geometry.GeometryId, id_B: pydrake.geometry.GeometryId) → drake::geometry::SignedDistancePair<double>

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().

Raises:
  • if either geometry id is invalid, or if the pair (id_A, id_B) has
  • been marked as filtered.
ComputeSignedDistancePairwiseClosestPoints(self: pydrake.geometry.QueryObject_[float], max_distance: float = inf) → List[drake::geometry::SignedDistancePair<double>]

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.

Scalar support

This function does not support halfspaces. If an unfiltered pair contains a halfspace, an exception will be thrown for all scalar types. Otherwise, this query supports all other pairs of Drake geometry types for double. For AutoDiffXd, it only supports distance between sphere-box and sphere-sphere. If there are any unfiltered geometry pairs that include other geometries, the AutoDiff throws an exception.

Parameter 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.
ComputeSignedDistanceToPoint(self: pydrake.geometry.QueryObject_[float], p_WQ: numpy.ndarray[numpy.float64[3, 1]], threshold: float = inf) → List[drake::geometry::SignedDistanceToPoint<double>]

Computes the signed distances and gradients to a query point from each geometry in the scene.

Warning

Currently supports spheres, boxes, and cylinders only. Silently ignores other kinds of geometries, which will be added later.

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.

Scalar support

This query only supports computing distances from the point to spheres, boxes, and cylinders for both double and AutoDiffXd scalar types. If the SceneGraph contains any other geometry shapes, they will be silently ignored.

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

Parameter p_WQ:
Position of a query point Q in world frame W.
Parameter threshold:
We ignore any object beyond this distance. By default, it is infinity, so we report distances from the query point to every object.
Returns signed_distances:
A vector populated with per-object signed distance values (and supporting data). See SignedDistanceToPoint.
FindCollisionCandidates(self: pydrake.geometry.QueryObject_[float]) → List[drake::SortedPair<drake::geometry::GeometryId>]

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

Warning

This silently ignores Mesh geometries (but Convex mesh geometries are included).

HasCollisions(self: pydrake.geometry.QueryObject_[float]) → bool

Reports true if there are any collisions between unfiltered pairs in the world.

Warning

This silently ignores Mesh geometries (but Convex mesh geometries are included).

inspector(self: pydrake.geometry.QueryObject_[float]) → pydrake.geometry.SceneGraphInspector_[float]

Provides an inspector for the topological structure of the underlying scene graph data (see SceneGraphInspector for details).

RenderColorImage(self: pydrake.geometry.QueryObject_[float], camera: drake::geometry::render::CameraProperties, parent_frame: pydrake.geometry.FrameId, X_PC: pydrake.math.RigidTransform_[float], show_window: bool = False) → drake::systems::sensors::Image<(drake::systems::sensors::PixelType)2>

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

Parameter camera:
The intrinsic properties of the camera.
Parameter parent_frame:
The id for the camera’s parent frame.
Parameter X_PC:
The pose of the camera body in the world frame.
Parameter show_window:
If true, the render window will be displayed.
Parameter color_image_out:
The rendered color image.
RenderDepthImage(self: pydrake.geometry.QueryObject_[float], camera: drake::geometry::render::DepthCameraProperties, parent_frame: pydrake.geometry.FrameId, X_PC: pydrake.math.RigidTransform_[float]) → drake::systems::sensors::Image<(drake::systems::sensors::PixelType)6>

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.

Parameter camera:
The intrinsic properties of the camera.
Parameter parent_frame:
The id for the camera’s parent frame.
Parameter X_PC:
The pose of the camera body in the world frame.
Parameter depth_image_out:
The rendered depth image.
RenderLabelImage(self: pydrake.geometry.QueryObject_[float], camera: drake::geometry::render::CameraProperties, parent_frame: pydrake.geometry.FrameId, X_PC: pydrake.math.RigidTransform_[float], show_window: bool = False) → drake::systems::sensors::Image<(drake::systems::sensors::PixelType)7>

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

Parameter camera:
The intrinsic properties of the camera.
Parameter parent_frame:
The id for the camera’s parent frame.
Parameter X_PC:
The pose of the camera body in the world frame.
Parameter show_window:
If true, the render window will be displayed.
Parameter label_image_out:
The rendered label image.
X_PF(self: pydrake.geometry.QueryObject_[float], id: pydrake.geometry.FrameId) → pydrake.math.RigidTransform_[float]

Reports the position of the frame indicated by 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 X_WF().

Note

This is analogous to but distinct from SceneGraphInspector::X_PG(). In this case, the pose will always be relative to another frame.

Raises:RuntimeError if the frame id is not valid.
X_WF(self: pydrake.geometry.QueryObject_[float], id: pydrake.geometry.FrameId) → pydrake.math.RigidTransform_[float]

Reports the position of the frame indicated by id relative to the world frame.

Raises:RuntimeError if the frame id is not valid.
X_WG(self: pydrake.geometry.QueryObject_[float], id: pydrake.geometry.GeometryId) → pydrake.math.RigidTransform_[float]

Reports the position of the geometry indicated by id relative to the world frame.

Raises:RuntimeError if the geometry id is not valid.
pydrake.geometry.ReadObjToSurfaceMesh(filename: str, scale: float = 1.0) → drake::geometry::SurfaceMesh<double>

Constructs a surface mesh from a Wavefront .obj file and optionally scales coordinates by the given scale factor. Polygons will be triangulated if they are not triangles already. All objects in the .obj file will be merged into the surface mesh. See https://en.wikipedia.org/wiki/Wavefront_.obj_file for the file format.

Parameter filename:
A valid file name with absolute path or relative path.
Parameter scale:
An optional scale to coordinates.
Raises:
  • RuntimeError if filename doesn’t have a valid file path, or
  • the file has no faces.
Returns:

surface mesh

class pydrake.geometry.Rgba

Defines RGBA (red, green, blue, alpha) values on the range [0, 1].

__init__(self: pydrake.geometry.Rgba, r: float, g: float, b: float, a: float = 1.0) → None

Constructs with given (r, g, b, a) values.

Precondition:
All values are within the range of [0, 1].
a(self: pydrake.geometry.Rgba) → float

Alpha.

b(self: pydrake.geometry.Rgba) → float

Blue.

g(self: pydrake.geometry.Rgba) → float

Green.

r(self: pydrake.geometry.Rgba) → float

Red.

set(self: pydrake.geometry.Rgba, r: float, g: float, b: float, a: float = 1.0) → None

Sets (r, g, b, a) values.

Precondition:
All values are within the range of [0, 1]. The values are not updated if this precondition is not met.
class pydrake.geometry.Role

General enumeration for indicating geometry role.

Members:

kUnassigned :

kProximity :

kIllustration :

kPerception :

__init__(self: pydrake.geometry.Role, arg0: int) → None
kIllustration = Role.kIllustration
kPerception = Role.kPerception
kProximity = Role.kProximity
kUnassigned = Role.kUnassigned
name
class pydrake.geometry.RoleAssign

The operations that can be performed on the given properties when assigning roles to geometry.

Members:

kNew : Assign the properties to a geometry that doesn’t already have the

role.

kReplace : Replace the existing role properties completely.
__init__(self: pydrake.geometry.RoleAssign, arg0: int) → None
kNew = RoleAssign.kNew
kReplace = RoleAssign.kReplace
name
pydrake.geometry.SceneGraph

alias of pydrake.geometry.SceneGraph_[float]

template pydrake.geometry.SceneGraph_

Instantiations: SceneGraph_[float], SceneGraph_[AutoDiffXd]

class SceneGraph_[float]

Bases: pydrake.systems.framework.LeafSystem_[float]

SceneGraph serves as the nexus for all geometry (and geometry-based operations) in a Diagram. Through SceneGraph, other systems that introduce geometry can register that geometry as part of a common global domain, including it in geometric queries (e.g., cars controlled by one LeafSystem can be observed by a different sensor system). SceneGraph provides the interface for registering the geometry, updating its position based on the current context, and performing geometric queries.

source_pose{0}→
...→
source_pose{N-1}→
SceneGraph
→ lcm_visualization
→ query

Only registered “geometry sources” can introduce geometry into SceneGraph. Geometry sources will typically be other leaf systems, but, in the case of anchored (i.e., stationary) geometry, it could also be some other block of code (e.g., adding a common ground plane with which all systems’ geometries interact). For dynamic geometry (geometry whose pose depends on a Context), the geometry source must also provide pose values for all of the geometries the source owns, via a port connection on SceneGraph. For N geometry sources, the SceneGraph instance will have N pose input ports.

The basic workflow for interacting with SceneGraph is:

  • Register as a geometry source, acquiring a unique SourceId.
  • Register geometry (anchored and dynamic) with the system.
  • Connect source’s geometry output ports to the corresponding SceneGraph

input ports. - Implement appropriate Calc* methods on the geometry output ports to update geometry pose values.

For each registered geometry source, there is one input port for each order of kinematics values (e.g., pose, velocity, and acceleration). If a source registers a frame, it must connect to these ports (although, in the current version, only pose is supported). Failure to connect to the port (or to provide valid kinematics values) will lead to runtime exceptions.

pose port: An abstract-valued port providing an instance of FramePoseVector. For each registered frame, this “pose vector” maps the registered FrameId to a pose value. All registered frames must be accounted for and only frames registered by a source can be included in its output port. See the details in FrameKinematicsVector for details on how to provide values for this port.

SceneGraph has two output ports:

query port: An abstract-valued port containing an instance of QueryObject. It provides a “ticket” for downstream LeafSystem instances to perform geometric queries on the SceneGraph. To perform geometric queries, downstream LeafSystem instances acquire the QueryObject from SceneGraph’s output port and provide it as a parameter to one of SceneGraph’s query methods (e.g., SceneGraph::ComputeContact()). This assumes that the querying system has access to a const pointer to the connected SceneGraph instance. Use get_query_output_port() to acquire the output port for the query handle.

lcm visualization port: An abstract-valued port containing an instance of PoseBundle. This is a convenience port designed to feed LCM update messages to drake_visualizer for the purpose of visualizing the state of the world’s geometry. Additional uses of this port are strongly discouraged; instead, use an appropriate geometric query to obtain the state of the world’s geometry.

LeafSystem instances can relate to SceneGraph in one of two ways: as a consumer that performs queries, or as a producer that introduces geometry into the shared world and defines its context-dependent kinematics values. It is reasonable for systems to perform either role singly, or both.

Consumer

Consumers perform geometric queries upon the world geometry. SceneGraph serves those queries. As indicated above, in order for a LeafSystem to act as a consumer, it must: 1. define a QueryObject-valued input port and connect it to SceneGraph’s corresponding output port, and 2. have a reference to the connected SceneGraph instance.

With those two requirements satisfied, a LeafSystem can perform geometry queries by: 1. evaluating the QueryObject input port, and 2. passing the returned query object into the appropriate query method on SceneGraph (e.g., SceneGraph::ComputeContact()).

Producer

All producers introduce geometry into the shared geometric world. This is called registering geometry. Depending on what exactly has been registered, a producer may also have to update kinematics. Producers themselves must be registered with SceneGraph as producers (a.k.a. geometry sources). They do this by acquiring a SourceId (via SceneGraph::RegisterSource()). The SourceId serves as a unique handle through which the producer’s identity is validated and its ownership of its registered geometry is maintained.

Registering Geometry

SceneGraph cannot know what geometry should be part of the shared world. Other systems are responsible for introducing geometry into the world. This process (defining geometry and informing SceneGraph) is called registering the geometry. The source that registers the geometry “owns” the geometry; the source’s unique SourceId is required to perform any operations on the geometry registered with that SourceId. Geometry can be registered as anchored or dynamic.

Dynamic geometry can move; more specifically, its kinematics (e.g., pose) depends on a system’s Context. Particularly, dynamic geometry is fixed to a frame whose kinematics values depend on a context. As the frame moves, the geometries fixed to it move with it. Therefore, to register dynamic geometry a frame must be registered first. These registered frames serve as the basis for repositioning geometry in the shared world. The geometry source is responsible for providing up-to-date kinematics values for those registered frames upon request (via an appropriate output port on the source LeafSystem connecting to the appropriate input port on SceneGraph). The work flow is as follows: 1. A LeafSystem registers itself as a geometry source, acquiring a SourceId (RegisterSource()). 2. The source registers a frame (GeometrySource::RegisterFrame()). - A frame always has a “parent” frame. It can implicitly be the world frame, or another frame registered by the source. 3. Register one or more geometries to a frame (GeometrySource::RegisterGeometry()). - The registered geometry is posed relative to the frame to which it is fixed. - The geometry can also be posed relative to another registered geometry. It will be affixed to that geometry’s frame.

Anchored geometry is independent of the context (i.e., it doesn’t move). Anchored geometries are always affixed to the immobile world frame. As such, registering a frame is not required for registering anchored geometry (see GeometrySource::RegisterAnchoredGeometry()). However, the source still “owns” the anchored geometry.

Updating Kinematics

Registering dynamic geometry implies a contract between the geometry source and SceneGraph. The geometry source must do the following: - It must provide, populate, and connect two output ports: the “id” port and the “pose” port. - The id port must contain all the frame ids returned as a result of frame registration. - The pose port must contain one pose per registered frame; the pose value is expressed relative to the registered frame’s parent frame. As mentioned above, the iᵗʰ pose value should describe the frame indicated by the iᵗʰ id in the id output port.

Failure to meet these requirements will lead to a run-time error.

Many (and eventually all) methods that configure the population of SceneGraph have two variants that differ by whether they accept a mutable Context or not. When no Context is provided, this SceneGraph instance’s underlying model is modified. When the SceneGraph instance allocates a context, its model is copied into that context.

The second variant causes SceneGraph to modify the data stored in the provided Context to be modified instead of the internal model.

The two interfaces can be used interchangeably. However, modifications to this SceneGraph’s underlying model will not affect previously allocated Context instances. A new Context should be allocated after modifying the model.

Note

In this initial version, the only methods with the Context-modifying variant are those methods that do not change the the semantics of the input or output ports. Modifications that make such changes must be coordinated across systems.

__init__(self: pydrake.geometry.SceneGraph_[float]) → None

Constructs a default (empty) scene graph.

AddRenderer(self: pydrake.geometry.SceneGraph_[float], name: str, renderer: drake::geometry::render::RenderEngine) → None

Adds a new render engine to this SceneGraph. The SceneGraph owns the render engine. The render engine’s name should be referenced in the render::CameraProperties “CameraProperties” provided in the render queries (see QueryObject::RenderColorImage() as an example).

There is no restriction on when a renderer is added relative to geometry registration and role assignment. Given a representative sequence of registration and perception role assignment, the addition of the renderer can be introduced anywhere in the sequence and the end result would be the same.

GeometryId id1 = scene_graph.RegisterGeometry(source_id, ...);
scene_graph.AssignRole(source_id, id1, PerceptionProperties());
GeometryId id2 = scene_graph.RegisterGeometry(source_id, ...);
scene_graph.AssignRole(source_id, id2, PerceptionProperties());
GeometryId id3 = scene_graph.RegisterGeometry(source_id, ...);
scene_graph.AssignRole(source_id, id3, PerceptionProperties());
Parameter name:
The unique name of the renderer.
Parameter renderer:
The renderer to add.
Raises:RuntimeError if the name is not unique.
AssignRole(*args, **kwargs)

Overloaded function.

  1. AssignRole(self: pydrake.geometry.SceneGraph_[float], source_id: pydrake.geometry.SourceId, geometry_id: pydrake.geometry.GeometryId, properties: pydrake.geometry.ProximityProperties, assign: pydrake.geometry.RoleAssign = RoleAssign.kNew) -> None

Assigns the proximity role to the geometry indicated by geometry_id.

  1. AssignRole(self: pydrake.geometry.SceneGraph_[float], context: pydrake.systems.framework.Context_[float], source_id: pydrake.geometry.SourceId, geometry_id: pydrake.geometry.GeometryId, properties: pydrake.geometry.ProximityProperties, assign: pydrake.geometry.RoleAssign = RoleAssign.kNew) -> None

systems::Context-modifying variant of AssignRole(SourceId,GeometryId,ProximityProperties) “AssignRole()” for proximity properties. Rather than modifying SceneGraph’s model, it modifies the copy of the model stored in the provided context.

  1. AssignRole(self: pydrake.geometry.SceneGraph_[float], source_id: pydrake.geometry.SourceId, geometry_id: pydrake.geometry.GeometryId, properties: pydrake.geometry.PerceptionProperties, assign: pydrake.geometry.RoleAssign = RoleAssign.kNew) -> None

Assigns the perception role to the geometry indicated by geometry_id.

By default, a geometry with a perception role will be reified by all render::RenderEngine instances. This behavior can be changed. Renderers can be explicitly whitelisted via the (‘renderer’, ‘accepting’) perception property. Its type is std::set<std::string> and it contains the names of all the renderers that may reify it. If no property is defined (or an empty set is given), then the default behavior of all renderers attempting to reify it will be restored.

  1. AssignRole(self: pydrake.geometry.SceneGraph_[float], context: pydrake.systems.framework.Context_[float], source_id: pydrake.geometry.SourceId, geometry_id: pydrake.geometry.GeometryId, properties: pydrake.geometry.PerceptionProperties, assign: pydrake.geometry.RoleAssign = RoleAssign.kNew) -> None

systems::Context-modifying variant of AssignRole(SourceId,GeometryId,PerceptionProperties) “AssignRole()” for perception properties. Rather than modifying SceneGraph’s model, it modifies the copy of the model stored in the provided context.

  1. AssignRole(self: pydrake.geometry.SceneGraph_[float], source_id: pydrake.geometry.SourceId, geometry_id: pydrake.geometry.GeometryId, properties: pydrake.geometry.IllustrationProperties, assign: pydrake.geometry.RoleAssign = RoleAssign.kNew) -> None

Assigns the illustration role to the geometry indicated by geometry_id.

Warning

When changing illustration properties (assign = RoleAssign::kReplace), there is no guarantee that these changes will affect the visualization. The visualizer needs to be able to “initialize” itself after changes to properties that will affect how a geometry appears. If changing a geometry’s illustration properties doesn’t seem to be affecting the visualization, retrigger its initialization action.

  1. AssignRole(self: pydrake.geometry.SceneGraph_[float], context: pydrake.systems.framework.Context_[float], source_id: pydrake.geometry.SourceId, geometry_id: pydrake.geometry.GeometryId, properties: pydrake.geometry.IllustrationProperties, assign: pydrake.geometry.RoleAssign = RoleAssign.kNew) -> None

systems::Context-modifying variant of AssignRole(SourceId,GeometryId,IllustrationProperties) “AssignRole()” for illustration properties. Rather than modifying SceneGraph’s model, it modifies the copy of the model stored in the provided context.

Warning

When changing illustration properties (assign = RoleAssign::kReplace), there is no guarantee that these changes will affect the visualization. The visualizer needs to be able to “initialize” itself after changes to properties that will affect how a geometry appears. If changing a geometry’s illustration properties doesn’t seem to be affecting the visualization, retrigger its initialization action.

Warning

Due to a bug (see issue #13597), changing the illustration roles or properties in a systems::Context will not have any apparent effect in, at least, drake_visualizer. Please change the illustration role in the model prior to allocating the context.

ExcludeCollisionsBetween(*args, **kwargs)

Overloaded function.

  1. ExcludeCollisionsBetween(self: pydrake.geometry.SceneGraph_[float], setA: pydrake.geometry.GeometrySet, setB: pydrake.geometry.GeometrySet) -> None

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 = {a₀, a₁, ..., aₘ} and B = {b₀, b₁, ..., bₙ} are the input sets of geometries setA and setB, respectively. This does not preclude collisions between members of the same set.

See also

scene_graph_collision_filtering for requirements and how collision filtering works.

Raises:
  • RuntimeError if the groups include ids that don’t exist in the
  • scene graph.
  1. ExcludeCollisionsBetween(self: pydrake.geometry.SceneGraph_[float], context: pydrake.systems.framework.Context_[float], setA: pydrake.geometry.GeometrySet, setB: pydrake.geometry.GeometrySet) -> None

systems::Context-modifying variant of ExcludeCollisionsBetween(). Rather than modifying SceneGraph’s model, it modifies the copy of the model stored in the provided context.

ExcludeCollisionsWithin(*args, **kwargs)

Overloaded function.

  1. ExcludeCollisionsWithin(self: pydrake.geometry.SceneGraph_[float], set: pydrake.geometry.GeometrySet) -> None

Excludes geometry pairs from collision evaluation by updating the candidate pair set C = C - P, where P = {(gᵢ, gⱼ)}, gᵢ, gⱼ G and G = {g₀, g₁, ..., gₘ} is the input set of geometries.

This method modifies the underlying model and requires a new Context to be allocated.

See also

scene_graph_collision_filtering for requirements and how collision filtering works.

Raises:
  • RuntimeError if the set includes ids that don’t exist in the scene
  • graph.
  1. ExcludeCollisionsWithin(self: pydrake.geometry.SceneGraph_[float], context: pydrake.systems.framework.Context_[float], set: pydrake.geometry.GeometrySet) -> None

systems::Context-modifying variant of ExcludeCollisionsWithin(). Rather than modifying SceneGraph’s model, it modifies the copy of the model stored in the provided context.

get_pose_bundle_output_port(self: pydrake.geometry.SceneGraph_[float]) → pydrake.systems.framework.OutputPort_[float]

Returns the output port which produces the PoseBundle for LCM communication to drake visualizer.

get_query_output_port(self: pydrake.geometry.SceneGraph_[float]) → pydrake.systems.framework.OutputPort_[float]

Returns the output port which produces the QueryObject for performing geometric queries.

get_source_pose_port(self: pydrake.geometry.SceneGraph_[float], arg0: pydrake.geometry.SourceId) → pydrake.systems.framework.InputPort_[float]

Given a valid source id, returns a pose input port associated with that id. This port is used to communicate pose data for registered frames.

Raises:RuntimeError if the source_id is not recognized.
HasRenderer(self: pydrake.geometry.SceneGraph_[float], name: str) → bool

Reports if this SceneGraph has a renderer registered to the given name.

model_inspector(self: pydrake.geometry.SceneGraph_[float]) → pydrake.geometry.SceneGraphInspector_[float]

Returns an inspector on the system’s model scene graph data.

RegisterAnchoredGeometry(self: pydrake.geometry.SceneGraph_[float], source_id: pydrake.geometry.SourceId, geometry: pydrake.geometry.GeometryInstance) → pydrake.geometry.GeometryId

Registers a new anchored geometry G for this source. This hangs geometry G from the world frame (W). Its pose is defined in that frame (i.e., X_WG). Returns the corresponding unique geometry id.

Roles will be assigned to the registered geometry if the corresponding GeometryInstance geometry has had properties assigned.

This method modifies the underlying model and requires a new Context to be allocated.

Parameter source_id:
The id for the source registering the frame.
Parameter geometry:
The anchored geometry G to add to the world.
Returns:

A unique identifier for the added geometry.

Raises:
  • RuntimeError if a) the source_id does not map to a
  • registered source or b) the geometry’s name doesn’t satisfy the
  • requirements outlined in GeometryInstance.
RegisterFrame(*args, **kwargs)

Overloaded function.

  1. RegisterFrame(self: pydrake.geometry.SceneGraph_[float], source_id: pydrake.geometry.SourceId, frame: pydrake.geometry.GeometryFrame) -> pydrake.geometry.FrameId

Registers a new frame F for this source. This hangs frame F on the world frame (W). Its pose is defined relative to the world frame (i.e, X_WF). Returns the corresponding unique frame id.

This method modifies the underlying model and requires a new Context to be allocated.

Parameter source_id:
The id for the source registering the frame.
Parameter frame:
The frame to register.
Returns:

A unique identifier for the added frame.

Raises:
  • RuntimeError if a) the source_id does not map to a
  • registered source, or b) frame has an id that has already been
  • registered.
  1. RegisterFrame(self: pydrake.geometry.SceneGraph_[float], source_id: pydrake.geometry.SourceId, parent_id: pydrake.geometry.FrameId, frame: pydrake.geometry.GeometryFrame) -> pydrake.geometry.FrameId

Registers a new frame F for this source. This hangs frame F on another previously registered frame P (indicated by parent_id). The pose of the new frame is defined relative to the parent frame (i.e., X_PF). Returns the corresponding unique frame id.

This method modifies the underlying model and requires a new Context to be allocated.

Parameter source_id:
The id for the source registering the frame.
Parameter parent_id:
The id of the parent frame P.
Parameter frame:
The frame to register.
Returns:

A unique identifier for the added frame.

Raises:
  • RuntimeError if a) the source_id does not map to a
  • registered source, b) the parent_id does not map to a known
  • frame or does not belong to the source, or c) frame has an id
  • that has already been registered.
RegisterGeometry(*args, **kwargs)

Overloaded function.

  1. RegisterGeometry(self: pydrake.geometry.SceneGraph_[float], source_id: pydrake.geometry.SourceId, frame_id: pydrake.geometry.FrameId, geometry: pydrake.geometry.GeometryInstance) -> pydrake.geometry.GeometryId

Registers a new geometry G for this source. This hangs geometry G on a previously registered frame F (indicated by frame_id). The pose of the geometry is defined in a fixed pose relative to F (i.e., X_FG). Returns the corresponding unique geometry id.

Roles will be assigned to the registered geometry if the corresponding GeometryInstance geometry has had properties assigned.

This method modifies the underlying model and requires a new Context to be allocated.

Parameter source_id:
The id for the source registering the geometry.
Parameter frame_id:
The id for the frame F to hang the geometry on.
Parameter geometry:
The geometry G to affix to frame F.
Returns:

A unique identifier for the added geometry.

Raises:
  • RuntimeError if a) the source_id does not map to a
  • registered source, b) the frame_id doesn’t belong to the
  • source, c) the geometry is equal to nullptr, or d) the
  • geometry’s name doesn’t satisfy the requirements outlined in
  • GeometryInstance.
  1. RegisterGeometry(self: pydrake.geometry.SceneGraph_[float], source_id: pydrake.geometry.SourceId, geometry_id: pydrake.geometry.GeometryId, geometry: pydrake.geometry.GeometryInstance) -> pydrake.geometry.GeometryId

Registers a new geometry G for this source. This hangs geometry G on a previously registered geometry P (indicated by geometry_id). The pose of the geometry is defined in a fixed pose relative to geometry P (i.e., X_PG). By induction, this geometry is effectively rigidly affixed to the frame that P is affixed to. Returns the corresponding unique geometry id.

Roles will be assigned to the registered geometry if the corresponding GeometryInstance geometry has had properties assigned.

This method modifies the underlying model and requires a new Context to be allocated.

Parameter source_id:
The id for the source registering the geometry.
Parameter geometry_id:
The id for the parent geometry P.
Parameter geometry:
The geometry G to add.
Returns:

A unique identifier for the added geometry.

Raises:
  • RuntimeError if a) the source_id does not map to a
  • registered source, b) the geometry_id doesn’t belong to the
  • source, c) the geometry is equal to nullptr, or d) the
  • geometry’s name doesn’t satisfy the requirements outlined in
  • GeometryInstance.
RegisterSource(self: pydrake.geometry.SceneGraph_[float], name: str = '') → pydrake.geometry.SourceId

Registers a new, named source to the geometry system. The caller must save the returned SourceId; it is the token by which all other operations on the geometry world are conducted.

This source id can be used to register arbitrary anchored geometry. But if dynamic geometry is registered (via RegisterGeometry/RegisterFrame), then the context-dependent pose values must be provided on an input port. See get_source_pose_port().

This method modifies the underlying model and requires a new Context to be allocated.

Parameter name:
The optional name of the source. If none is provided (or the empty string) a default name will be defined by SceneGraph’s logic.
Raises:RuntimeError if the name is not unique.
RemoveRole(self: pydrake.geometry.SceneGraph_[float], source_id: pydrake.geometry.SourceId, geometry_id: pydrake.geometry.GeometryId, role: pydrake.geometry.Role) → int

Removes the indicated role from the geometry indicated by geometry_id.

Returns:

One if the geometry had the role removed and zero if the geometry did not have the role assigned in the first place.

Raises:
  • RuntimeError if a) source_id does not map to a registered
  • source, b) geometry_id does not map to a registered geometry,
    1. geometry_id does not belong to source_id, or d) the
  • context has already been allocated.
RendererCount(self: pydrake.geometry.SceneGraph_[float]) → int

Reports the number of renderers registered to this SceneGraph.

static world_frame_id() → pydrake.geometry.FrameId

Reports the identifier for the world frame.

class pydrake.geometry.SceneGraph_[AutoDiffXd]

Bases: pydrake.systems.framework.LeafSystem_[AutoDiffXd]

SceneGraph serves as the nexus for all geometry (and geometry-based operations) in a Diagram. Through SceneGraph, other systems that introduce geometry can register that geometry as part of a common global domain, including it in geometric queries (e.g., cars controlled by one LeafSystem can be observed by a different sensor system). SceneGraph provides the interface for registering the geometry, updating its position based on the current context, and performing geometric queries.

source_pose{0}→
...→
source_pose{N-1}→
SceneGraph
→ lcm_visualization
→ query

Only registered “geometry sources” can introduce geometry into SceneGraph. Geometry sources will typically be other leaf systems, but, in the case of anchored (i.e., stationary) geometry, it could also be some other block of code (e.g., adding a common ground plane with which all systems’ geometries interact). For dynamic geometry (geometry whose pose depends on a Context), the geometry source must also provide pose values for all of the geometries the source owns, via a port connection on SceneGraph. For N geometry sources, the SceneGraph instance will have N pose input ports.

The basic workflow for interacting with SceneGraph is:

  • Register as a geometry source, acquiring a unique SourceId.
  • Register geometry (anchored and dynamic) with the system.
  • Connect source’s geometry output ports to the corresponding SceneGraph

input ports. - Implement appropriate Calc* methods on the geometry output ports to update geometry pose values.

For each registered geometry source, there is one input port for each order of kinematics values (e.g., pose, velocity, and acceleration). If a source registers a frame, it must connect to these ports (although, in the current version, only pose is supported). Failure to connect to the port (or to provide valid kinematics values) will lead to runtime exceptions.

pose port: An abstract-valued port providing an instance of FramePoseVector. For each registered frame, this “pose vector” maps the registered FrameId to a pose value. All registered frames must be accounted for and only frames registered by a source can be included in its output port. See the details in FrameKinematicsVector for details on how to provide values for this port.

SceneGraph has two output ports:

query port: An abstract-valued port containing an instance of QueryObject. It provides a “ticket” for downstream LeafSystem instances to perform geometric queries on the SceneGraph. To perform geometric queries, downstream LeafSystem instances acquire the QueryObject from SceneGraph’s output port and provide it as a parameter to one of SceneGraph’s query methods (e.g., SceneGraph::ComputeContact()). This assumes that the querying system has access to a const pointer to the connected SceneGraph instance. Use get_query_output_port() to acquire the output port for the query handle.

lcm visualization port: An abstract-valued port containing an instance of PoseBundle. This is a convenience port designed to feed LCM update messages to drake_visualizer for the purpose of visualizing the state of the world’s geometry. Additional uses of this port are strongly discouraged; instead, use an appropriate geometric query to obtain the state of the world’s geometry.

LeafSystem instances can relate to SceneGraph in one of two ways: as a consumer that performs queries, or as a producer that introduces geometry into the shared world and defines its context-dependent kinematics values. It is reasonable for systems to perform either role singly, or both.

Consumer

Consumers perform geometric queries upon the world geometry. SceneGraph serves those queries. As indicated above, in order for a LeafSystem to act as a consumer, it must: 1. define a QueryObject-valued input port and connect it to SceneGraph’s corresponding output port, and 2. have a reference to the connected SceneGraph instance.

With those two requirements satisfied, a LeafSystem can perform geometry queries by: 1. evaluating the QueryObject input port, and 2. passing the returned query object into the appropriate query method on SceneGraph (e.g., SceneGraph::ComputeContact()).

Producer

All producers introduce geometry into the shared geometric world. This is called registering geometry. Depending on what exactly has been registered, a producer may also have to update kinematics. Producers themselves must be registered with SceneGraph as producers (a.k.a. geometry sources). They do this by acquiring a SourceId (via SceneGraph::RegisterSource()). The SourceId serves as a unique handle through which the producer’s identity is validated and its ownership of its registered geometry is maintained.

Registering Geometry

SceneGraph cannot know what geometry should be part of the shared world. Other systems are responsible for introducing geometry into the world. This process (defining geometry and informing SceneGraph) is called registering the geometry. The source that registers the geometry “owns” the geometry; the source’s unique SourceId is required to perform any operations on the geometry registered with that SourceId. Geometry can be registered as anchored or dynamic.

Dynamic geometry can move; more specifically, its kinematics (e.g., pose) depends on a system’s Context. Particularly, dynamic geometry is fixed to a frame whose kinematics values depend on a context. As the frame moves, the geometries fixed to it move with it. Therefore, to register dynamic geometry a frame must be registered first. These registered frames serve as the basis for repositioning geometry in the shared world. The geometry source is responsible for providing up-to-date kinematics values for those registered frames upon request (via an appropriate output port on the source LeafSystem connecting to the appropriate input port on SceneGraph). The work flow is as follows: 1. A LeafSystem registers itself as a geometry source, acquiring a SourceId (RegisterSource()). 2. The source registers a frame (GeometrySource::RegisterFrame()). - A frame always has a “parent” frame. It can implicitly be the world frame, or another frame registered by the source. 3. Register one or more geometries to a frame (GeometrySource::RegisterGeometry()). - The registered geometry is posed relative to the frame to which it is fixed. - The geometry can also be posed relative to another registered geometry. It will be affixed to that geometry’s frame.

Anchored geometry is independent of the context (i.e., it doesn’t move). Anchored geometries are always affixed to the immobile world frame. As such, registering a frame is not required for registering anchored geometry (see GeometrySource::RegisterAnchoredGeometry()). However, the source still “owns” the anchored geometry.

Updating Kinematics

Registering dynamic geometry implies a contract between the geometry source and SceneGraph. The geometry source must do the following: - It must provide, populate, and connect two output ports: the “id” port and the “pose” port. - The id port must contain all the frame ids returned as a result of frame registration. - The pose port must contain one pose per registered frame; the pose value is expressed relative to the registered frame’s parent frame. As mentioned above, the iᵗʰ pose value should describe the frame indicated by the iᵗʰ id in the id output port.

Failure to meet these requirements will lead to a run-time error.

Many (and eventually all) methods that configure the population of SceneGraph have two variants that differ by whether they accept a mutable Context or not. When no Context is provided, this SceneGraph instance’s underlying model is modified. When the SceneGraph instance allocates a context, its model is copied into that context.

The second variant causes SceneGraph to modify the data stored in the provided Context to be modified instead of the internal model.

The two interfaces can be used interchangeably. However, modifications to this SceneGraph’s underlying model will not affect previously allocated Context instances. A new Context should be allocated after modifying the model.

Note

In this initial version, the only methods with the Context-modifying variant are those methods that do not change the the semantics of the input or output ports. Modifications that make such changes must be coordinated across systems.

__init__(self: pydrake.geometry.SceneGraph_[AutoDiffXd]) → None

Constructs a default (empty) scene graph.

AddRenderer(self: pydrake.geometry.SceneGraph_[AutoDiffXd], name: str, renderer: drake::geometry::render::RenderEngine) → None

Adds a new render engine to this SceneGraph. The SceneGraph owns the render engine. The render engine’s name should be referenced in the render::CameraProperties “CameraProperties” provided in the render queries (see QueryObject::RenderColorImage() as an example).

There is no restriction on when a renderer is added relative to geometry registration and role assignment. Given a representative sequence of registration and perception role assignment, the addition of the renderer can be introduced anywhere in the sequence and the end result would be the same.

GeometryId id1 = scene_graph.RegisterGeometry(source_id, ...);
scene_graph.AssignRole(source_id, id1, PerceptionProperties());
GeometryId id2 = scene_graph.RegisterGeometry(source_id, ...);
scene_graph.AssignRole(source_id, id2, PerceptionProperties());
GeometryId id3 = scene_graph.RegisterGeometry(source_id, ...);
scene_graph.AssignRole(source_id, id3, PerceptionProperties());
Parameter name:
The unique name of the renderer.
Parameter renderer:
The renderer to add.
Raises:RuntimeError if the name is not unique.
AssignRole(*args, **kwargs)

Overloaded function.

  1. AssignRole(self: pydrake.geometry.SceneGraph_[AutoDiffXd], source_id: pydrake.geometry.SourceId, geometry_id: pydrake.geometry.GeometryId, properties: pydrake.geometry.ProximityProperties, assign: pydrake.geometry.RoleAssign = RoleAssign.kNew) -> None

Assigns the proximity role to the geometry indicated by geometry_id.

  1. AssignRole(self: pydrake.geometry.SceneGraph_[AutoDiffXd], context: pydrake.systems.framework.Context_[AutoDiffXd], source_id: pydrake.geometry.SourceId, geometry_id: pydrake.geometry.GeometryId, properties: pydrake.geometry.ProximityProperties, assign: pydrake.geometry.RoleAssign = RoleAssign.kNew) -> None

systems::Context-modifying variant of AssignRole(SourceId,GeometryId,ProximityProperties) “AssignRole()” for proximity properties. Rather than modifying SceneGraph’s model, it modifies the copy of the model stored in the provided context.

  1. AssignRole(self: pydrake.geometry.SceneGraph_[AutoDiffXd], source_id: pydrake.geometry.SourceId, geometry_id: pydrake.geometry.GeometryId, properties: pydrake.geometry.PerceptionProperties, assign: pydrake.geometry.RoleAssign = RoleAssign.kNew) -> None

Assigns the perception role to the geometry indicated by geometry_id.

By default, a geometry with a perception role will be reified by all render::RenderEngine instances. This behavior can be changed. Renderers can be explicitly whitelisted via the (‘renderer’, ‘accepting’) perception property. Its type is std::set<std::string> and it contains the names of all the renderers that may reify it. If no property is defined (or an empty set is given), then the default behavior of all renderers attempting to reify it will be restored.

  1. AssignRole(self: pydrake.geometry.SceneGraph_[AutoDiffXd], context: pydrake.systems.framework.Context_[AutoDiffXd], source_id: pydrake.geometry.SourceId, geometry_id: pydrake.geometry.GeometryId, properties: pydrake.geometry.PerceptionProperties, assign: pydrake.geometry.RoleAssign = RoleAssign.kNew) -> None

systems::Context-modifying variant of AssignRole(SourceId,GeometryId,PerceptionProperties) “AssignRole()” for perception properties. Rather than modifying SceneGraph’s model, it modifies the copy of the model stored in the provided context.

  1. AssignRole(self: pydrake.geometry.SceneGraph_[AutoDiffXd], source_id: pydrake.geometry.SourceId, geometry_id: pydrake.geometry.GeometryId, properties: pydrake.geometry.IllustrationProperties, assign: pydrake.geometry.RoleAssign = RoleAssign.kNew) -> None

Assigns the illustration role to the geometry indicated by geometry_id.

Warning

When changing illustration properties (assign = RoleAssign::kReplace), there is no guarantee that these changes will affect the visualization. The visualizer needs to be able to “initialize” itself after changes to properties that will affect how a geometry appears. If changing a geometry’s illustration properties doesn’t seem to be affecting the visualization, retrigger its initialization action.

  1. AssignRole(self: pydrake.geometry.SceneGraph_[AutoDiffXd], context: pydrake.systems.framework.Context_[AutoDiffXd], source_id: pydrake.geometry.SourceId, geometry_id: pydrake.geometry.GeometryId, properties: pydrake.geometry.IllustrationProperties, assign: pydrake.geometry.RoleAssign = RoleAssign.kNew) -> None

systems::Context-modifying variant of AssignRole(SourceId,GeometryId,IllustrationProperties) “AssignRole()” for illustration properties. Rather than modifying SceneGraph’s model, it modifies the copy of the model stored in the provided context.

Warning

When changing illustration properties (assign = RoleAssign::kReplace), there is no guarantee that these changes will affect the visualization. The visualizer needs to be able to “initialize” itself after changes to properties that will affect how a geometry appears. If changing a geometry’s illustration properties doesn’t seem to be affecting the visualization, retrigger its initialization action.

Warning

Due to a bug (see issue #13597), changing the illustration roles or properties in a systems::Context will not have any apparent effect in, at least, drake_visualizer. Please change the illustration role in the model prior to allocating the context.

ExcludeCollisionsBetween(*args, **kwargs)

Overloaded function.

  1. ExcludeCollisionsBetween(self: pydrake.geometry.SceneGraph_[AutoDiffXd], setA: pydrake.geometry.GeometrySet, setB: pydrake.geometry.GeometrySet) -> None

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 = {a₀, a₁, ..., aₘ} and B = {b₀, b₁, ..., bₙ} are the input sets of geometries setA and setB, respectively. This does not preclude collisions between members of the same set.

See also

scene_graph_collision_filtering for requirements and how collision filtering works.

Raises:
  • RuntimeError if the groups include ids that don’t exist in the
  • scene graph.
  1. ExcludeCollisionsBetween(self: pydrake.geometry.SceneGraph_[AutoDiffXd], context: pydrake.systems.framework.Context_[AutoDiffXd], setA: pydrake.geometry.GeometrySet, setB: pydrake.geometry.GeometrySet) -> None

systems::Context-modifying variant of ExcludeCollisionsBetween(). Rather than modifying SceneGraph’s model, it modifies the copy of the model stored in the provided context.

ExcludeCollisionsWithin(*args, **kwargs)

Overloaded function.

  1. ExcludeCollisionsWithin(self: pydrake.geometry.SceneGraph_[AutoDiffXd], set: pydrake.geometry.GeometrySet) -> None

Excludes geometry pairs from collision evaluation by updating the candidate pair set C = C - P, where P = {(gᵢ, gⱼ)}, gᵢ, gⱼ G and G = {g₀, g₁, ..., gₘ} is the input set of geometries.

This method modifies the underlying model and requires a new Context to be allocated.

See also

scene_graph_collision_filtering for requirements and how collision filtering works.

Raises:
  • RuntimeError if the set includes ids that don’t exist in the scene
  • graph.
  1. ExcludeCollisionsWithin(self: pydrake.geometry.SceneGraph_[AutoDiffXd], context: pydrake.systems.framework.Context_[AutoDiffXd], set: pydrake.geometry.GeometrySet) -> None

systems::Context-modifying variant of ExcludeCollisionsWithin(). Rather than modifying SceneGraph’s model, it modifies the copy of the model stored in the provided context.

get_pose_bundle_output_port(self: pydrake.geometry.SceneGraph_[AutoDiffXd]) → pydrake.systems.framework.OutputPort_[AutoDiffXd]

Returns the output port which produces the PoseBundle for LCM communication to drake visualizer.

get_query_output_port(self: pydrake.geometry.SceneGraph_[AutoDiffXd]) → pydrake.systems.framework.OutputPort_[AutoDiffXd]

Returns the output port which produces the QueryObject for performing geometric queries.

get_source_pose_port(self: pydrake.geometry.SceneGraph_[AutoDiffXd], arg0: pydrake.geometry.SourceId) → pydrake.systems.framework.InputPort_[AutoDiffXd]

Given a valid source id, returns a pose input port associated with that id. This port is used to communicate pose data for registered frames.

Raises:RuntimeError if the source_id is not recognized.
HasRenderer(self: pydrake.geometry.SceneGraph_[AutoDiffXd], name: str) → bool

Reports if this SceneGraph has a renderer registered to the given name.

model_inspector(self: pydrake.geometry.SceneGraph_[AutoDiffXd]) → pydrake.geometry.SceneGraphInspector_[AutoDiffXd]

Returns an inspector on the system’s model scene graph data.

RegisterAnchoredGeometry(self: pydrake.geometry.SceneGraph_[AutoDiffXd], source_id: pydrake.geometry.SourceId, geometry: pydrake.geometry.GeometryInstance) → pydrake.geometry.GeometryId

Registers a new anchored geometry G for this source. This hangs geometry G from the world frame (W). Its pose is defined in that frame (i.e., X_WG). Returns the corresponding unique geometry id.

Roles will be assigned to the registered geometry if the corresponding GeometryInstance geometry has had properties assigned.

This method modifies the underlying model and requires a new Context to be allocated.

Parameter source_id:
The id for the source registering the frame.
Parameter geometry:
The anchored geometry G to add to the world.
Returns:

A unique identifier for the added geometry.

Raises:
  • RuntimeError if a) the source_id does not map to a
  • registered source or b) the geometry’s name doesn’t satisfy the
  • requirements outlined in GeometryInstance.
RegisterFrame(*args, **kwargs)

Overloaded function.

  1. RegisterFrame(self: pydrake.geometry.SceneGraph_[AutoDiffXd], source_id: pydrake.geometry.SourceId, frame: pydrake.geometry.GeometryFrame) -> pydrake.geometry.FrameId

Registers a new frame F for this source. This hangs frame F on the world frame (W). Its pose is defined relative to the world frame (i.e, X_WF). Returns the corresponding unique frame id.

This method modifies the underlying model and requires a new Context to be allocated.

Parameter source_id:
The id for the source registering the frame.
Parameter frame:
The frame to register.
Returns:

A unique identifier for the added frame.

Raises:
  • RuntimeError if a) the source_id does not map to a
  • registered source, or b) frame has an id that has already been
  • registered.
  1. RegisterFrame(self: pydrake.geometry.SceneGraph_[AutoDiffXd], source_id: pydrake.geometry.SourceId, parent_id: pydrake.geometry.FrameId, frame: pydrake.geometry.GeometryFrame) -> pydrake.geometry.FrameId

Registers a new frame F for this source. This hangs frame F on another previously registered frame P (indicated by parent_id). The pose of the new frame is defined relative to the parent frame (i.e., X_PF). Returns the corresponding unique frame id.

This method modifies the underlying model and requires a new Context to be allocated.

Parameter source_id:
The id for the source registering the frame.
Parameter parent_id:
The id of the parent frame P.
Parameter frame:
The frame to register.
Returns:

A unique identifier for the added frame.

Raises:
  • RuntimeError if a) the source_id does not map to a
  • registered source, b) the parent_id does not map to a known
  • frame or does not belong to the source, or c) frame has an id
  • that has already been registered.
RegisterGeometry(*args, **kwargs)

Overloaded function.

  1. RegisterGeometry(self: pydrake.geometry.SceneGraph_[AutoDiffXd], source_id: pydrake.geometry.SourceId, frame_id: pydrake.geometry.FrameId, geometry: pydrake.geometry.GeometryInstance) -> pydrake.geometry.GeometryId

Registers a new geometry G for this source. This hangs geometry G on a previously registered frame F (indicated by frame_id). The pose of the geometry is defined in a fixed pose relative to F (i.e., X_FG). Returns the corresponding unique geometry id.

Roles will be assigned to the registered geometry if the corresponding GeometryInstance geometry has had properties assigned.

This method modifies the underlying model and requires a new Context to be allocated.

Parameter source_id:
The id for the source registering the geometry.
Parameter frame_id:
The id for the frame F to hang the geometry on.
Parameter geometry:
The geometry G to affix to frame F.
Returns:

A unique identifier for the added geometry.

Raises:
  • RuntimeError if a) the source_id does not map to a
  • registered source, b) the frame_id doesn’t belong to the
  • source, c) the geometry is equal to nullptr, or d) the
  • geometry’s name doesn’t satisfy the requirements outlined in
  • GeometryInstance.
  1. RegisterGeometry(self: pydrake.geometry.SceneGraph_[AutoDiffXd], source_id: pydrake.geometry.SourceId, geometry_id: pydrake.geometry.GeometryId, geometry: pydrake.geometry.GeometryInstance) -> pydrake.geometry.GeometryId

Registers a new geometry G for this source. This hangs geometry G on a previously registered geometry P (indicated by geometry_id). The pose of the geometry is defined in a fixed pose relative to geometry P (i.e., X_PG). By induction, this geometry is effectively rigidly affixed to the frame that P is affixed to. Returns the corresponding unique geometry id.

Roles will be assigned to the registered geometry if the corresponding GeometryInstance geometry has had properties assigned.

This method modifies the underlying model and requires a new Context to be allocated.

Parameter source_id:
The id for the source registering the geometry.
Parameter geometry_id:
The id for the parent geometry P.
Parameter geometry:
The geometry G to add.
Returns:

A unique identifier for the added geometry.

Raises:
  • RuntimeError if a) the source_id does not map to a
  • registered source, b) the geometry_id doesn’t belong to the
  • source, c) the geometry is equal to nullptr, or d) the
  • geometry’s name doesn’t satisfy the requirements outlined in
  • GeometryInstance.
RegisterSource(self: pydrake.geometry.SceneGraph_[AutoDiffXd], name: str = '') → pydrake.geometry.SourceId

Registers a new, named source to the geometry system. The caller must save the returned SourceId; it is the token by which all other operations on the geometry world are conducted.

This source id can be used to register arbitrary anchored geometry. But if dynamic geometry is registered (via RegisterGeometry/RegisterFrame), then the context-dependent pose values must be provided on an input port. See get_source_pose_port().

This method modifies the underlying model and requires a new Context to be allocated.

Parameter name:
The optional name of the source. If none is provided (or the empty string) a default name will be defined by SceneGraph’s logic.
Raises:RuntimeError if the name is not unique.
RemoveRole(self: pydrake.geometry.SceneGraph_[AutoDiffXd], source_id: pydrake.geometry.SourceId, geometry_id: pydrake.geometry.GeometryId, role: pydrake.geometry.Role) → int

Removes the indicated role from the geometry indicated by geometry_id.

Returns:

One if the geometry had the role removed and zero if the geometry did not have the role assigned in the first place.

Raises:
  • RuntimeError if a) source_id does not map to a registered
  • source, b) geometry_id does not map to a registered geometry,
    1. geometry_id does not belong to source_id, or d) the
  • context has already been allocated.
RendererCount(self: pydrake.geometry.SceneGraph_[AutoDiffXd]) → int

Reports the number of renderers registered to this SceneGraph.

static world_frame_id() → pydrake.geometry.FrameId

Reports the identifier for the world frame.

class pydrake.geometry.SceneGraph_[float]

Bases: pydrake.systems.framework.LeafSystem_[float]

SceneGraph serves as the nexus for all geometry (and geometry-based operations) in a Diagram. Through SceneGraph, other systems that introduce geometry can register that geometry as part of a common global domain, including it in geometric queries (e.g., cars controlled by one LeafSystem can be observed by a different sensor system). SceneGraph provides the interface for registering the geometry, updating its position based on the current context, and performing geometric queries.

source_pose{0}→
...→
source_pose{N-1}→
SceneGraph
→ lcm_visualization
→ query

Only registered “geometry sources” can introduce geometry into SceneGraph. Geometry sources will typically be other leaf systems, but, in the case of anchored (i.e., stationary) geometry, it could also be some other block of code (e.g., adding a common ground plane with which all systems’ geometries interact). For dynamic geometry (geometry whose pose depends on a Context), the geometry source must also provide pose values for all of the geometries the source owns, via a port connection on SceneGraph. For N geometry sources, the SceneGraph instance will have N pose input ports.

The basic workflow for interacting with SceneGraph is:

  • Register as a geometry source, acquiring a unique SourceId.
  • Register geometry (anchored and dynamic) with the system.
  • Connect source’s geometry output ports to the corresponding SceneGraph

input ports. - Implement appropriate Calc* methods on the geometry output ports to update geometry pose values.

For each registered geometry source, there is one input port for each order of kinematics values (e.g., pose, velocity, and acceleration). If a source registers a frame, it must connect to these ports (although, in the current version, only pose is supported). Failure to connect to the port (or to provide valid kinematics values) will lead to runtime exceptions.

pose port: An abstract-valued port providing an instance of FramePoseVector. For each registered frame, this “pose vector” maps the registered FrameId to a pose value. All registered frames must be accounted for and only frames registered by a source can be included in its output port. See the details in FrameKinematicsVector for details on how to provide values for this port.

SceneGraph has two output ports:

query port: An abstract-valued port containing an instance of QueryObject. It provides a “ticket” for downstream LeafSystem instances to perform geometric queries on the SceneGraph. To perform geometric queries, downstream LeafSystem instances acquire the QueryObject from SceneGraph’s output port and provide it as a parameter to one of SceneGraph’s query methods (e.g., SceneGraph::ComputeContact()). This assumes that the querying system has access to a const pointer to the connected SceneGraph instance. Use get_query_output_port() to acquire the output port for the query handle.

lcm visualization port: An abstract-valued port containing an instance of PoseBundle. This is a convenience port designed to feed LCM update messages to drake_visualizer for the purpose of visualizing the state of the world’s geometry. Additional uses of this port are strongly discouraged; instead, use an appropriate geometric query to obtain the state of the world’s geometry.

LeafSystem instances can relate to SceneGraph in one of two ways: as a consumer that performs queries, or as a producer that introduces geometry into the shared world and defines its context-dependent kinematics values. It is reasonable for systems to perform either role singly, or both.

Consumer

Consumers perform geometric queries upon the world geometry. SceneGraph serves those queries. As indicated above, in order for a LeafSystem to act as a consumer, it must: 1. define a QueryObject-valued input port and connect it to SceneGraph’s corresponding output port, and 2. have a reference to the connected SceneGraph instance.

With those two requirements satisfied, a LeafSystem can perform geometry queries by: 1. evaluating the QueryObject input port, and 2. passing the returned query object into the appropriate query method on SceneGraph (e.g., SceneGraph::ComputeContact()).

Producer

All producers introduce geometry into the shared geometric world. This is called registering geometry. Depending on what exactly has been registered, a producer may also have to update kinematics. Producers themselves must be registered with SceneGraph as producers (a.k.a. geometry sources). They do this by acquiring a SourceId (via SceneGraph::RegisterSource()). The SourceId serves as a unique handle through which the producer’s identity is validated and its ownership of its registered geometry is maintained.

Registering Geometry

SceneGraph cannot know what geometry should be part of the shared world. Other systems are responsible for introducing geometry into the world. This process (defining geometry and informing SceneGraph) is called registering the geometry. The source that registers the geometry “owns” the geometry; the source’s unique SourceId is required to perform any operations on the geometry registered with that SourceId. Geometry can be registered as anchored or dynamic.

Dynamic geometry can move; more specifically, its kinematics (e.g., pose) depends on a system’s Context. Particularly, dynamic geometry is fixed to a frame whose kinematics values depend on a context. As the frame moves, the geometries fixed to it move with it. Therefore, to register dynamic geometry a frame must be registered first. These registered frames serve as the basis for repositioning geometry in the shared world. The geometry source is responsible for providing up-to-date kinematics values for those registered frames upon request (via an appropriate output port on the source LeafSystem connecting to the appropriate input port on SceneGraph). The work flow is as follows: 1. A LeafSystem registers itself as a geometry source, acquiring a SourceId (RegisterSource()). 2. The source registers a frame (GeometrySource::RegisterFrame()). - A frame always has a “parent” frame. It can implicitly be the world frame, or another frame registered by the source. 3. Register one or more geometries to a frame (GeometrySource::RegisterGeometry()). - The registered geometry is posed relative to the frame to which it is fixed. - The geometry can also be posed relative to another registered geometry. It will be affixed to that geometry’s frame.

Anchored geometry is independent of the context (i.e., it doesn’t move). Anchored geometries are always affixed to the immobile world frame. As such, registering a frame is not required for registering anchored geometry (see GeometrySource::RegisterAnchoredGeometry()). However, the source still “owns” the anchored geometry.

Updating Kinematics

Registering dynamic geometry implies a contract between the geometry source and SceneGraph. The geometry source must do the following: - It must provide, populate, and connect two output ports: the “id” port and the “pose” port. - The id port must contain all the frame ids returned as a result of frame registration. - The pose port must contain one pose per registered frame; the pose value is expressed relative to the registered frame’s parent frame. As mentioned above, the iᵗʰ pose value should describe the frame indicated by the iᵗʰ id in the id output port.

Failure to meet these requirements will lead to a run-time error.

Many (and eventually all) methods that configure the population of SceneGraph have two variants that differ by whether they accept a mutable Context or not. When no Context is provided, this SceneGraph instance’s underlying model is modified. When the SceneGraph instance allocates a context, its model is copied into that context.

The second variant causes SceneGraph to modify the data stored in the provided Context to be modified instead of the internal model.

The two interfaces can be used interchangeably. However, modifications to this SceneGraph’s underlying model will not affect previously allocated Context instances. A new Context should be allocated after modifying the model.

Note

In this initial version, the only methods with the Context-modifying variant are those methods that do not change the the semantics of the input or output ports. Modifications that make such changes must be coordinated across systems.

__init__(self: pydrake.geometry.SceneGraph_[float]) → None

Constructs a default (empty) scene graph.

AddRenderer(self: pydrake.geometry.SceneGraph_[float], name: str, renderer: drake::geometry::render::RenderEngine) → None

Adds a new render engine to this SceneGraph. The SceneGraph owns the render engine. The render engine’s name should be referenced in the render::CameraProperties “CameraProperties” provided in the render queries (see QueryObject::RenderColorImage() as an example).

There is no restriction on when a renderer is added relative to geometry registration and role assignment. Given a representative sequence of registration and perception role assignment, the addition of the renderer can be introduced anywhere in the sequence and the end result would be the same.

GeometryId id1 = scene_graph.RegisterGeometry(source_id, ...);
scene_graph.AssignRole(source_id, id1, PerceptionProperties());
GeometryId id2 = scene_graph.RegisterGeometry(source_id, ...);
scene_graph.AssignRole(source_id, id2, PerceptionProperties());
GeometryId id3 = scene_graph.RegisterGeometry(source_id, ...);
scene_graph.AssignRole(source_id, id3, PerceptionProperties());
Parameter name:
The unique name of the renderer.
Parameter renderer:
The renderer to add.
Raises:RuntimeError if the name is not unique.
AssignRole(*args, **kwargs)

Overloaded function.

  1. AssignRole(self: pydrake.geometry.SceneGraph_[float], source_id: pydrake.geometry.SourceId, geometry_id: pydrake.geometry.GeometryId, properties: pydrake.geometry.ProximityProperties, assign: pydrake.geometry.RoleAssign = RoleAssign.kNew) -> None

Assigns the proximity role to the geometry indicated by geometry_id.

  1. AssignRole(self: pydrake.geometry.SceneGraph_[float], context: pydrake.systems.framework.Context_[float], source_id: pydrake.geometry.SourceId, geometry_id: pydrake.geometry.GeometryId, properties: pydrake.geometry.ProximityProperties, assign: pydrake.geometry.RoleAssign = RoleAssign.kNew) -> None

systems::Context-modifying variant of AssignRole(SourceId,GeometryId,ProximityProperties) “AssignRole()” for proximity properties. Rather than modifying SceneGraph’s model, it modifies the copy of the model stored in the provided context.

  1. AssignRole(self: pydrake.geometry.SceneGraph_[float], source_id: pydrake.geometry.SourceId, geometry_id: pydrake.geometry.GeometryId, properties: pydrake.geometry.PerceptionProperties, assign: pydrake.geometry.RoleAssign = RoleAssign.kNew) -> None

Assigns the perception role to the geometry indicated by geometry_id.

By default, a geometry with a perception role will be reified by all render::RenderEngine instances. This behavior can be changed. Renderers can be explicitly whitelisted via the (‘renderer’, ‘accepting’) perception property. Its type is std::set<std::string> and it contains the names of all the renderers that may reify it. If no property is defined (or an empty set is given), then the default behavior of all renderers attempting to reify it will be restored.

  1. AssignRole(self: pydrake.geometry.SceneGraph_[float], context: pydrake.systems.framework.Context_[float], source_id: pydrake.geometry.SourceId, geometry_id: pydrake.geometry.GeometryId, properties: pydrake.geometry.PerceptionProperties, assign: pydrake.geometry.RoleAssign = RoleAssign.kNew) -> None

systems::Context-modifying variant of AssignRole(SourceId,GeometryId,PerceptionProperties) “AssignRole()” for perception properties. Rather than modifying SceneGraph’s model, it modifies the copy of the model stored in the provided context.

  1. AssignRole(self: pydrake.geometry.SceneGraph_[float], source_id: pydrake.geometry.SourceId, geometry_id: pydrake.geometry.GeometryId, properties: pydrake.geometry.IllustrationProperties, assign: pydrake.geometry.RoleAssign = RoleAssign.kNew) -> None

Assigns the illustration role to the geometry indicated by geometry_id.

Warning

When changing illustration properties (assign = RoleAssign::kReplace), there is no guarantee that these changes will affect the visualization. The visualizer needs to be able to “initialize” itself after changes to properties that will affect how a geometry appears. If changing a geometry’s illustration properties doesn’t seem to be affecting the visualization, retrigger its initialization action.

  1. AssignRole(self: pydrake.geometry.SceneGraph_[float], context: pydrake.systems.framework.Context_[float], source_id: pydrake.geometry.SourceId, geometry_id: pydrake.geometry.GeometryId, properties: pydrake.geometry.IllustrationProperties, assign: pydrake.geometry.RoleAssign = RoleAssign.kNew) -> None

systems::Context-modifying variant of AssignRole(SourceId,GeometryId,IllustrationProperties) “AssignRole()” for illustration properties. Rather than modifying SceneGraph’s model, it modifies the copy of the model stored in the provided context.

Warning

When changing illustration properties (assign = RoleAssign::kReplace), there is no guarantee that these changes will affect the visualization. The visualizer needs to be able to “initialize” itself after changes to properties that will affect how a geometry appears. If changing a geometry’s illustration properties doesn’t seem to be affecting the visualization, retrigger its initialization action.

Warning

Due to a bug (see issue #13597), changing the illustration roles or properties in a systems::Context will not have any apparent effect in, at least, drake_visualizer. Please change the illustration role in the model prior to allocating the context.

ExcludeCollisionsBetween(*args, **kwargs)

Overloaded function.

  1. ExcludeCollisionsBetween(self: pydrake.geometry.SceneGraph_[float], setA: pydrake.geometry.GeometrySet, setB: pydrake.geometry.GeometrySet) -> None

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 = {a₀, a₁, ..., aₘ} and B = {b₀, b₁, ..., bₙ} are the input sets of geometries setA and setB, respectively. This does not preclude collisions between members of the same set.

See also

scene_graph_collision_filtering for requirements and how collision filtering works.

Raises:
  • RuntimeError if the groups include ids that don’t exist in the
  • scene graph.
  1. ExcludeCollisionsBetween(self: pydrake.geometry.SceneGraph_[float], context: pydrake.systems.framework.Context_[float], setA: pydrake.geometry.GeometrySet, setB: pydrake.geometry.GeometrySet) -> None

systems::Context-modifying variant of ExcludeCollisionsBetween(). Rather than modifying SceneGraph’s model, it modifies the copy of the model stored in the provided context.

ExcludeCollisionsWithin(*args, **kwargs)

Overloaded function.

  1. ExcludeCollisionsWithin(self: pydrake.geometry.SceneGraph_[float], set: pydrake.geometry.GeometrySet) -> None

Excludes geometry pairs from collision evaluation by updating the candidate pair set C = C - P, where P = {(gᵢ, gⱼ)}, gᵢ, gⱼ G and G = {g₀, g₁, ..., gₘ} is the input set of geometries.

This method modifies the underlying model and requires a new Context to be allocated.

See also

scene_graph_collision_filtering for requirements and how collision filtering works.

Raises:
  • RuntimeError if the set includes ids that don’t exist in the scene
  • graph.
  1. ExcludeCollisionsWithin(self: pydrake.geometry.SceneGraph_[float], context: pydrake.systems.framework.Context_[float], set: pydrake.geometry.GeometrySet) -> None

systems::Context-modifying variant of ExcludeCollisionsWithin(). Rather than modifying SceneGraph’s model, it modifies the copy of the model stored in the provided context.

get_pose_bundle_output_port(self: pydrake.geometry.SceneGraph_[float]) → pydrake.systems.framework.OutputPort_[float]

Returns the output port which produces the PoseBundle for LCM communication to drake visualizer.

get_query_output_port(self: pydrake.geometry.SceneGraph_[float]) → pydrake.systems.framework.OutputPort_[float]

Returns the output port which produces the QueryObject for performing geometric queries.

get_source_pose_port(self: pydrake.geometry.SceneGraph_[float], arg0: pydrake.geometry.SourceId) → pydrake.systems.framework.InputPort_[float]

Given a valid source id, returns a pose input port associated with that id. This port is used to communicate pose data for registered frames.

Raises:RuntimeError if the source_id is not recognized.
HasRenderer(self: pydrake.geometry.SceneGraph_[float], name: str) → bool

Reports if this SceneGraph has a renderer registered to the given name.

model_inspector(self: pydrake.geometry.SceneGraph_[float]) → pydrake.geometry.SceneGraphInspector_[float]

Returns an inspector on the system’s model scene graph data.

RegisterAnchoredGeometry(self: pydrake.geometry.SceneGraph_[float], source_id: pydrake.geometry.SourceId, geometry: pydrake.geometry.GeometryInstance) → pydrake.geometry.GeometryId

Registers a new anchored geometry G for this source. This hangs geometry G from the world frame (W). Its pose is defined in that frame (i.e., X_WG). Returns the corresponding unique geometry id.

Roles will be assigned to the registered geometry if the corresponding GeometryInstance geometry has had properties assigned.

This method modifies the underlying model and requires a new Context to be allocated.

Parameter source_id:
The id for the source registering the frame.
Parameter geometry:
The anchored geometry G to add to the world.
Returns:

A unique identifier for the added geometry.

Raises:
  • RuntimeError if a) the source_id does not map to a
  • registered source or b) the geometry’s name doesn’t satisfy the
  • requirements outlined in GeometryInstance.
RegisterFrame(*args, **kwargs)

Overloaded function.

  1. RegisterFrame(self: pydrake.geometry.SceneGraph_[float], source_id: pydrake.geometry.SourceId, frame: pydrake.geometry.GeometryFrame) -> pydrake.geometry.FrameId

Registers a new frame F for this source. This hangs frame F on the world frame (W). Its pose is defined relative to the world frame (i.e, X_WF). Returns the corresponding unique frame id.

This method modifies the underlying model and requires a new Context to be allocated.

Parameter source_id:
The id for the source registering the frame.
Parameter frame:
The frame to register.
Returns:

A unique identifier for the added frame.

Raises:
  • RuntimeError if a) the source_id does not map to a
  • registered source, or b) frame has an id that has already been
  • registered.
  1. RegisterFrame(self: pydrake.geometry.SceneGraph_[float], source_id: pydrake.geometry.SourceId, parent_id: pydrake.geometry.FrameId, frame: pydrake.geometry.GeometryFrame) -> pydrake.geometry.FrameId

Registers a new frame F for this source. This hangs frame F on another previously registered frame P (indicated by parent_id). The pose of the new frame is defined relative to the parent frame (i.e., X_PF). Returns the corresponding unique frame id.

This method modifies the underlying model and requires a new Context to be allocated.

Parameter source_id:
The id for the source registering the frame.
Parameter parent_id:
The id of the parent frame P.
Parameter frame:
The frame to register.
Returns:

A unique identifier for the added frame.

Raises:
  • RuntimeError if a) the source_id does not map to a
  • registered source, b) the parent_id does not map to a known
  • frame or does not belong to the source, or c) frame has an id
  • that has already been registered.
RegisterGeometry(*args, **kwargs)

Overloaded function.

  1. RegisterGeometry(self: pydrake.geometry.SceneGraph_[float], source_id: pydrake.geometry.SourceId, frame_id: pydrake.geometry.FrameId, geometry: pydrake.geometry.GeometryInstance) -> pydrake.geometry.GeometryId

Registers a new geometry G for this source. This hangs geometry G on a previously registered frame F (indicated by frame_id). The pose of the geometry is defined in a fixed pose relative to F (i.e., X_FG). Returns the corresponding unique geometry id.

Roles will be assigned to the registered geometry if the corresponding GeometryInstance geometry has had properties assigned.

This method modifies the underlying model and requires a new Context to be allocated.

Parameter source_id:
The id for the source registering the geometry.
Parameter frame_id:
The id for the frame F to hang the geometry on.
Parameter geometry:
The geometry G to affix to frame F.
Returns:

A unique identifier for the added geometry.

Raises:
  • RuntimeError if a) the source_id does not map to a
  • registered source, b) the frame_id doesn’t belong to the
  • source, c) the geometry is equal to nullptr, or d) the
  • geometry’s name doesn’t satisfy the requirements outlined in
  • GeometryInstance.
  1. RegisterGeometry(self: pydrake.geometry.SceneGraph_[float], source_id: pydrake.geometry.SourceId, geometry_id: pydrake.geometry.GeometryId, geometry: pydrake.geometry.GeometryInstance) -> pydrake.geometry.GeometryId

Registers a new geometry G for this source. This hangs geometry G on a previously registered geometry P (indicated by geometry_id). The pose of the geometry is defined in a fixed pose relative to geometry P (i.e., X_PG). By induction, this geometry is effectively rigidly affixed to the frame that P is affixed to. Returns the corresponding unique geometry id.

Roles will be assigned to the registered geometry if the corresponding GeometryInstance geometry has had properties assigned.

This method modifies the underlying model and requires a new Context to be allocated.

Parameter source_id:
The id for the source registering the geometry.
Parameter geometry_id:
The id for the parent geometry P.
Parameter geometry:
The geometry G to add.
Returns:

A unique identifier for the added geometry.

Raises:
  • RuntimeError if a) the source_id does not map to a
  • registered source, b) the geometry_id doesn’t belong to the
  • source, c) the geometry is equal to nullptr, or d) the
  • geometry’s name doesn’t satisfy the requirements outlined in
  • GeometryInstance.
RegisterSource(self: pydrake.geometry.SceneGraph_[float], name: str = '') → pydrake.geometry.SourceId

Registers a new, named source to the geometry system. The caller must save the returned SourceId; it is the token by which all other operations on the geometry world are conducted.

This source id can be used to register arbitrary anchored geometry. But if dynamic geometry is registered (via RegisterGeometry/RegisterFrame), then the context-dependent pose values must be provided on an input port. See get_source_pose_port().

This method modifies the underlying model and requires a new Context to be allocated.

Parameter name:
The optional name of the source. If none is provided (or the empty string) a default name will be defined by SceneGraph’s logic.
Raises:RuntimeError if the name is not unique.
RemoveRole(self: pydrake.geometry.SceneGraph_[float], source_id: pydrake.geometry.SourceId, geometry_id: pydrake.geometry.GeometryId, role: pydrake.geometry.Role) → int

Removes the indicated role from the geometry indicated by geometry_id.

Returns:

One if the geometry had the role removed and zero if the geometry did not have the role assigned in the first place.

Raises:
  • RuntimeError if a) source_id does not map to a registered
  • source, b) geometry_id does not map to a registered geometry,
    1. geometry_id does not belong to source_id, or d) the
  • context has already been allocated.
RendererCount(self: pydrake.geometry.SceneGraph_[float]) → int

Reports the number of renderers registered to this SceneGraph.

static world_frame_id() → pydrake.geometry.FrameId

Reports the identifier for the world frame.

pydrake.geometry.SceneGraphInspector

alias of pydrake.geometry.SceneGraphInspector_[float]

template pydrake.geometry.SceneGraphInspector_

Instantiations: SceneGraphInspector_[float], SceneGraphInspector_[AutoDiffXd]

class SceneGraphInspector_[float]

The SceneGraphInspector serves as a mechanism to query the topological structure of a SceneGraph instance. The topological structure consists of all of the SceneGraph data that does not depend on input pose data. Including, but not limited to:

  • names of frames and geometries
  • hierarchies (parents of geometries, parents of frames, etc.)
  • geometry parameters (e.g., contact, rendering, visualization)
  • fixed poses of geometries relative to frames

In contrast, the following pieces of data do depend on input pose data and cannot be performed with the SceneGraphInspector (see the QueryObject instead):

  • world pose of frames or geometry
  • collision queries
  • proximity queries

A SceneGraphInspector cannot be instantiated explicitly. Nor can it be copied or moved. A reference to a SceneGraphInspector instance can be acquired from

  • a SceneGraph instance (to inspect the state of the system’s model), or
  • a QueryObject instance (to inspect the state of the scene graph data stored

in the context).

The reference should not be persisted (and, as previously indicated, cannot be copied). SceneGraphInspector instances are cheap; they can be created, queried, and thrown out. If there is any doubt about the valid lifespan of a SceneGraphInspector, throw out the old instance and request a new instance.

Template parameter T:
The scalar of the associated SceneGraph instance. The template parameter is provided for the sake of compatibility, although no queries (or their results) depend on the scalar.
__init__

Initialize self. See help(type(self)) for accurate signature.

CloneGeometryInstance(self: pydrake.geometry.SceneGraphInspector_[float], geometry_id: pydrake.geometry.GeometryId) → pydrake.geometry.GeometryInstance

Obtains a new GeometryInstance that copies the geometry indicated by the given id.

Returns:A new GeometryInstance that is ready to be added as a new geometry. All roles/properties will be copied, the shape will be cloned based off of the original, but the returned id() will completely unique.
Raises:RuntimeError if the id does not refer to a valid geometry.
GetAllGeometryIds(self: pydrake.geometry.SceneGraphInspector_[float]) → List[pydrake.geometry.GeometryId]

Returns the set of all ids for registered geometries. The order is not guaranteed to have any particular meaning. But the order is guaranteed to remain fixed until a topological change is made (e.g., removal or addition of geometry/frames).

GetFrameId(self: pydrake.geometry.SceneGraphInspector_[float], geometry_id: pydrake.geometry.GeometryId) → pydrake.geometry.FrameId

Reports the id of the frame to which the given geometry with the given id is registered.

Raises:RuntimeError if id does not map to a registered geometry.
GetGeometryIdByName(self: pydrake.geometry.SceneGraphInspector_[float], frame_id: pydrake.geometry.FrameId, role: pydrake.geometry.Role, name: str) → pydrake.geometry.GeometryId

Reports the id of the geometry with the given name and role, attached to the frame with the given frame id.

Parameter id:
The id of the frame whose geometry is being queried.
Parameter role:
The assigned role of the desired geometry.
Parameter name:
The name of the geometry to query for. The name will be canonicalized prior to lookup (see canonicalized_geometry_names “GeometryInstance” for details).
Returns:

The id of the queried geometry.

Raises:
  • RuntimeError if no such geometry exists, multiple geometries have
  • that name, or if the id does not map to a registered frame.
GetIllustrationProperties(self: pydrake.geometry.SceneGraphInspector_[float], geometry_id: pydrake.geometry.GeometryId) → pydrake.geometry.IllustrationProperties

Returns a pointer to the const illustration properties of the geometry with the given id.

Parameter id:
The identifier for the queried geometry.
Returns:A pointer to the properties (or nullptr if there are no such properties).
Raises:RuntimeError if id does not map to a registered geometry.
GetName(*args, **kwargs)

Overloaded function.

  1. GetName(self: pydrake.geometry.SceneGraphInspector_[float], frame_id: pydrake.geometry.FrameId) -> str

Reports the name of the frame with the given id.

Raises:RuntimeError if id does not map to a registered frame.
  1. GetName(self: pydrake.geometry.SceneGraphInspector_[float], geometry_id: pydrake.geometry.GeometryId) -> str

Reports the stored, canonical name of the geometry with the given id (see canonicalized_geometry_names “GeometryInstance” for details).

Raises:RuntimeError if id does not map to a registered geometry.
GetNameByFrameId(self: pydrake.geometry.SceneGraphInspector_[float], frame_id: pydrake.geometry.FrameId) → str

Please use SceneGraphInspector.GetName() instead. This method will be removed on or after 2020-10-01.

GetNameByGeometryId(self: pydrake.geometry.SceneGraphInspector_[float], geometry_id: pydrake.geometry.GeometryId) → str

Please use SceneGraphInspector.GetName() instead. This method will be removed on or after 2020-10-01.

GetPerceptionProperties(self: pydrake.geometry.SceneGraphInspector_[float], geometry_id: pydrake.geometry.GeometryId) → pydrake.geometry.PerceptionProperties

Returns a pointer to the const perception properties of the geometry with the given id.

Parameter id:
The identifier for the queried geometry.
Returns:A pointer to the properties (or nullptr if there are no such properties).
Raises:RuntimeError if id does not map to a registered geometry.
GetPoseInFrame(self: pydrake.geometry.SceneGraphInspector_[float], geometry_id: pydrake.geometry.GeometryId) → pydrake.math.RigidTransform_[float]

Reports the pose of the geometry G with the given id in its registered frame F (regardless of whether its topological parent is another geometry P or not). If the geometry was registered directly to the frame F, then X_PG = X_FG.

See also

GetPoseInParent()

Raises:RuntimeError if id does not map to a registered geometry.
GetPoseInParent(self: pydrake.geometry.SceneGraphInspector_[float], geometry_id: pydrake.geometry.GeometryId) → pydrake.math.RigidTransform_[float]

Reports the pose of the geometry G with the given id in its registered topological parent P, X_PG. That topological parent may be a frame F or another geometry. If the geometry was registered directly to F, then X_PG = X_FG.

See also

GetPoseInFrame()

Raises:RuntimeError if id does not map to a registered geometry.
GetProximityProperties(self: pydrake.geometry.SceneGraphInspector_[float], geometry_id: pydrake.geometry.GeometryId) → pydrake.geometry.ProximityProperties

Returns a pointer to the const proximity properties of the geometry with the given id.

Parameter id:
The identifier for the queried geometry.
Returns:A pointer to the properties (or nullptr if there are no such properties).
Raises:RuntimeError if id does not map to a registered geometry.
GetShape(self: pydrake.geometry.SceneGraphInspector_[float], geometry_id: pydrake.geometry.GeometryId) → pydrake.geometry.Shape

Returns the shape specified for the geometry with the given id. In order to extract the details of the shape, it should be passed through an implementation of a ShapeReifier.

GetSourceName(self: pydrake.geometry.SceneGraphInspector_[float], id: pydrake.geometry.SourceId) → str

Reports the name for the source with the given id.

Raises:RuntimeError if id does not map to a registered source.
num_frames(self: pydrake.geometry.SceneGraphInspector_[float]) → int

Reports the total number of frames registered in the scene graph (including the world frame).

num_geometries(self: pydrake.geometry.SceneGraphInspector_[float]) → int

Reports the total number of geometries in the scene graph.

num_sources(self: pydrake.geometry.SceneGraphInspector_[float]) → int

Reports the number of registered sources – whether they have registered frames/geometries or not. This will always be at least 1; the SceneGraph itself counts as a source.

NumAnchoredGeometries(self: pydrake.geometry.SceneGraphInspector_[float]) → int

Reports the total number of anchored geometries. This should provide the same answer as calling NumGeometriesForFrame() with the world frame id.

NumDynamicGeometries(self: pydrake.geometry.SceneGraphInspector_[float]) → int

Reports the total number of dynamic geometries in the scene graph.

NumGeometriesWithRole(self: pydrake.geometry.SceneGraphInspector_[float], role: pydrake.geometry.Role) → int

Reports the total number of geometries in the scene graph with the indicated role.

SourceIsRegistered(self: pydrake.geometry.SceneGraphInspector_[float], id: pydrake.geometry.SourceId) → bool

Reports True if the given id maps to a registered source.

class pydrake.geometry.SceneGraphInspector_[AutoDiffXd]

The SceneGraphInspector serves as a mechanism to query the topological structure of a SceneGraph instance. The topological structure consists of all of the SceneGraph data that does not depend on input pose data. Including, but not limited to:

  • names of frames and geometries
  • hierarchies (parents of geometries, parents of frames, etc.)
  • geometry parameters (e.g., contact, rendering, visualization)
  • fixed poses of geometries relative to frames

In contrast, the following pieces of data do depend on input pose data and cannot be performed with the SceneGraphInspector (see the QueryObject instead):

  • world pose of frames or geometry
  • collision queries
  • proximity queries

A SceneGraphInspector cannot be instantiated explicitly. Nor can it be copied or moved. A reference to a SceneGraphInspector instance can be acquired from

  • a SceneGraph instance (to inspect the state of the system’s model), or
  • a QueryObject instance (to inspect the state of the scene graph data stored

in the context).

The reference should not be persisted (and, as previously indicated, cannot be copied). SceneGraphInspector instances are cheap; they can be created, queried, and thrown out. If there is any doubt about the valid lifespan of a SceneGraphInspector, throw out the old instance and request a new instance.

Template parameter T:
The scalar of the associated SceneGraph instance. The template parameter is provided for the sake of compatibility, although no queries (or their results) depend on the scalar.
__init__

Initialize self. See help(type(self)) for accurate signature.

CloneGeometryInstance(self: pydrake.geometry.SceneGraphInspector_[AutoDiffXd], geometry_id: pydrake.geometry.GeometryId) → pydrake.geometry.GeometryInstance

Obtains a new GeometryInstance that copies the geometry indicated by the given id.

Returns:A new GeometryInstance that is ready to be added as a new geometry. All roles/properties will be copied, the shape will be cloned based off of the original, but the returned id() will completely unique.
Raises:RuntimeError if the id does not refer to a valid geometry.
GetAllGeometryIds(self: pydrake.geometry.SceneGraphInspector_[AutoDiffXd]) → List[pydrake.geometry.GeometryId]

Returns the set of all ids for registered geometries. The order is not guaranteed to have any particular meaning. But the order is guaranteed to remain fixed until a topological change is made (e.g., removal or addition of geometry/frames).

GetFrameId(self: pydrake.geometry.SceneGraphInspector_[AutoDiffXd], geometry_id: pydrake.geometry.GeometryId) → pydrake.geometry.FrameId

Reports the id of the frame to which the given geometry with the given id is registered.

Raises:RuntimeError if id does not map to a registered geometry.
GetGeometryIdByName(self: pydrake.geometry.SceneGraphInspector_[AutoDiffXd], frame_id: pydrake.geometry.FrameId, role: pydrake.geometry.Role, name: str) → pydrake.geometry.GeometryId

Reports the id of the geometry with the given name and role, attached to the frame with the given frame id.

Parameter id:
The id of the frame whose geometry is being queried.
Parameter role:
The assigned role of the desired geometry.
Parameter name:
The name of the geometry to query for. The name will be canonicalized prior to lookup (see canonicalized_geometry_names “GeometryInstance” for details).
Returns:

The id of the queried geometry.

Raises:
  • RuntimeError if no such geometry exists, multiple geometries have
  • that name, or if the id does not map to a registered frame.
GetIllustrationProperties(self: pydrake.geometry.SceneGraphInspector_[AutoDiffXd], geometry_id: pydrake.geometry.GeometryId) → pydrake.geometry.IllustrationProperties

Returns a pointer to the const illustration properties of the geometry with the given id.

Parameter id:
The identifier for the queried geometry.
Returns:A pointer to the properties (or nullptr if there are no such properties).
Raises:RuntimeError if id does not map to a registered geometry.
GetName(*args, **kwargs)

Overloaded function.

  1. GetName(self: pydrake.geometry.SceneGraphInspector_[AutoDiffXd], frame_id: pydrake.geometry.FrameId) -> str

Reports the name of the frame with the given id.

Raises:RuntimeError if id does not map to a registered frame.
  1. GetName(self: pydrake.geometry.SceneGraphInspector_[AutoDiffXd], geometry_id: pydrake.geometry.GeometryId) -> str

Reports the stored, canonical name of the geometry with the given id (see canonicalized_geometry_names “GeometryInstance” for details).

Raises:RuntimeError if id does not map to a registered geometry.
GetNameByFrameId(self: pydrake.geometry.SceneGraphInspector_[AutoDiffXd], frame_id: pydrake.geometry.FrameId) → str

Please use SceneGraphInspector.GetName() instead. This method will be removed on or after 2020-10-01.

GetNameByGeometryId(self: pydrake.geometry.SceneGraphInspector_[AutoDiffXd], geometry_id: pydrake.geometry.GeometryId) → str

Please use SceneGraphInspector.GetName() instead. This method will be removed on or after 2020-10-01.

GetPerceptionProperties(self: pydrake.geometry.SceneGraphInspector_[AutoDiffXd], geometry_id: pydrake.geometry.GeometryId) → pydrake.geometry.PerceptionProperties

Returns a pointer to the const perception properties of the geometry with the given id.

Parameter id:
The identifier for the queried geometry.
Returns:A pointer to the properties (or nullptr if there are no such properties).
Raises:RuntimeError if id does not map to a registered geometry.
GetPoseInFrame(self: pydrake.geometry.SceneGraphInspector_[AutoDiffXd], geometry_id: pydrake.geometry.GeometryId) → pydrake.math.RigidTransform_[float]

Reports the pose of the geometry G with the given id in its registered frame F (regardless of whether its topological parent is another geometry P or not). If the geometry was registered directly to the frame F, then X_PG = X_FG.

See also

GetPoseInParent()

Raises:RuntimeError if id does not map to a registered geometry.
GetPoseInParent(self: pydrake.geometry.SceneGraphInspector_[AutoDiffXd], geometry_id: pydrake.geometry.GeometryId) → pydrake.math.RigidTransform_[float]

Reports the pose of the geometry G with the given id in its registered topological parent P, X_PG. That topological parent may be a frame F or another geometry. If the geometry was registered directly to F, then X_PG = X_FG.

See also

GetPoseInFrame()

Raises:RuntimeError if id does not map to a registered geometry.
GetProximityProperties(self: pydrake.geometry.SceneGraphInspector_[AutoDiffXd], geometry_id: pydrake.geometry.GeometryId) → pydrake.geometry.ProximityProperties

Returns a pointer to the const proximity properties of the geometry with the given id.

Parameter id:
The identifier for the queried geometry.
Returns:A pointer to the properties (or nullptr if there are no such properties).
Raises:RuntimeError if id does not map to a registered geometry.
GetShape(self: pydrake.geometry.SceneGraphInspector_[AutoDiffXd], geometry_id: pydrake.geometry.GeometryId) → pydrake.geometry.Shape

Returns the shape specified for the geometry with the given id. In order to extract the details of the shape, it should be passed through an implementation of a ShapeReifier.

GetSourceName(self: pydrake.geometry.SceneGraphInspector_[AutoDiffXd], id: pydrake.geometry.SourceId) → str

Reports the name for the source with the given id.

Raises:RuntimeError if id does not map to a registered source.
num_frames(self: pydrake.geometry.SceneGraphInspector_[AutoDiffXd]) → int

Reports the total number of frames registered in the scene graph (including the world frame).

num_geometries(self: pydrake.geometry.SceneGraphInspector_[AutoDiffXd]) → int

Reports the total number of geometries in the scene graph.

num_sources(self: pydrake.geometry.SceneGraphInspector_[AutoDiffXd]) → int

Reports the number of registered sources – whether they have registered frames/geometries or not. This will always be at least 1; the SceneGraph itself counts as a source.

NumAnchoredGeometries(self: pydrake.geometry.SceneGraphInspector_[AutoDiffXd]) → int

Reports the total number of anchored geometries. This should provide the same answer as calling NumGeometriesForFrame() with the world frame id.

NumDynamicGeometries(self: pydrake.geometry.SceneGraphInspector_[AutoDiffXd]) → int

Reports the total number of dynamic geometries in the scene graph.

NumGeometriesWithRole(self: pydrake.geometry.SceneGraphInspector_[AutoDiffXd], role: pydrake.geometry.Role) → int

Reports the total number of geometries in the scene graph with the indicated role.

SourceIsRegistered(self: pydrake.geometry.SceneGraphInspector_[AutoDiffXd], id: pydrake.geometry.SourceId) → bool

Reports True if the given id maps to a registered source.

class pydrake.geometry.SceneGraphInspector_[float]

The SceneGraphInspector serves as a mechanism to query the topological structure of a SceneGraph instance. The topological structure consists of all of the SceneGraph data that does not depend on input pose data. Including, but not limited to:

  • names of frames and geometries
  • hierarchies (parents of geometries, parents of frames, etc.)
  • geometry parameters (e.g., contact, rendering, visualization)
  • fixed poses of geometries relative to frames

In contrast, the following pieces of data do depend on input pose data and cannot be performed with the SceneGraphInspector (see the QueryObject instead):

  • world pose of frames or geometry
  • collision queries
  • proximity queries

A SceneGraphInspector cannot be instantiated explicitly. Nor can it be copied or moved. A reference to a SceneGraphInspector instance can be acquired from

  • a SceneGraph instance (to inspect the state of the system’s model), or
  • a QueryObject instance (to inspect the state of the scene graph data stored

in the context).

The reference should not be persisted (and, as previously indicated, cannot be copied). SceneGraphInspector instances are cheap; they can be created, queried, and thrown out. If there is any doubt about the valid lifespan of a SceneGraphInspector, throw out the old instance and request a new instance.

Template parameter T:
The scalar of the associated SceneGraph instance. The template parameter is provided for the sake of compatibility, although no queries (or their results) depend on the scalar.
__init__

Initialize self. See help(type(self)) for accurate signature.

CloneGeometryInstance(self: pydrake.geometry.SceneGraphInspector_[float], geometry_id: pydrake.geometry.GeometryId) → pydrake.geometry.GeometryInstance

Obtains a new GeometryInstance that copies the geometry indicated by the given id.

Returns:A new GeometryInstance that is ready to be added as a new geometry. All roles/properties will be copied, the shape will be cloned based off of the original, but the returned id() will completely unique.
Raises:RuntimeError if the id does not refer to a valid geometry.
GetAllGeometryIds(self: pydrake.geometry.SceneGraphInspector_[float]) → List[pydrake.geometry.GeometryId]

Returns the set of all ids for registered geometries. The order is not guaranteed to have any particular meaning. But the order is guaranteed to remain fixed until a topological change is made (e.g., removal or addition of geometry/frames).

GetFrameId(self: pydrake.geometry.SceneGraphInspector_[float], geometry_id: pydrake.geometry.GeometryId) → pydrake.geometry.FrameId

Reports the id of the frame to which the given geometry with the given id is registered.

Raises:RuntimeError if id does not map to a registered geometry.
GetGeometryIdByName(self: pydrake.geometry.SceneGraphInspector_[float], frame_id: pydrake.geometry.FrameId, role: pydrake.geometry.Role, name: str) → pydrake.geometry.GeometryId

Reports the id of the geometry with the given name and role, attached to the frame with the given frame id.

Parameter id:
The id of the frame whose geometry is being queried.
Parameter role:
The assigned role of the desired geometry.
Parameter name:
The name of the geometry to query for. The name will be canonicalized prior to lookup (see canonicalized_geometry_names “GeometryInstance” for details).
Returns:

The id of the queried geometry.

Raises:
  • RuntimeError if no such geometry exists, multiple geometries have
  • that name, or if the id does not map to a registered frame.
GetIllustrationProperties(self: pydrake.geometry.SceneGraphInspector_[float], geometry_id: pydrake.geometry.GeometryId) → pydrake.geometry.IllustrationProperties

Returns a pointer to the const illustration properties of the geometry with the given id.

Parameter id:
The identifier for the queried geometry.
Returns:A pointer to the properties (or nullptr if there are no such properties).
Raises:RuntimeError if id does not map to a registered geometry.
GetName(*args, **kwargs)

Overloaded function.

  1. GetName(self: pydrake.geometry.SceneGraphInspector_[float], frame_id: pydrake.geometry.FrameId) -> str

Reports the name of the frame with the given id.

Raises:RuntimeError if id does not map to a registered frame.
  1. GetName(self: pydrake.geometry.SceneGraphInspector_[float], geometry_id: pydrake.geometry.GeometryId) -> str

Reports the stored, canonical name of the geometry with the given id (see canonicalized_geometry_names “GeometryInstance” for details).

Raises:RuntimeError if id does not map to a registered geometry.
GetNameByFrameId(self: pydrake.geometry.SceneGraphInspector_[float], frame_id: pydrake.geometry.FrameId) → str

Please use SceneGraphInspector.GetName() instead. This method will be removed on or after 2020-10-01.

GetNameByGeometryId(self: pydrake.geometry.SceneGraphInspector_[float], geometry_id: pydrake.geometry.GeometryId) → str

Please use SceneGraphInspector.GetName() instead. This method will be removed on or after 2020-10-01.

GetPerceptionProperties(self: pydrake.geometry.SceneGraphInspector_[float], geometry_id: pydrake.geometry.GeometryId) → pydrake.geometry.PerceptionProperties

Returns a pointer to the const perception properties of the geometry with the given id.

Parameter id:
The identifier for the queried geometry.
Returns:A pointer to the properties (or nullptr if there are no such properties).
Raises:RuntimeError if id does not map to a registered geometry.
GetPoseInFrame(self: pydrake.geometry.SceneGraphInspector_[float], geometry_id: pydrake.geometry.GeometryId) → pydrake.math.RigidTransform_[float]

Reports the pose of the geometry G with the given id in its registered frame F (regardless of whether its topological parent is another geometry P or not). If the geometry was registered directly to the frame F, then X_PG = X_FG.

See also

GetPoseInParent()

Raises:RuntimeError if id does not map to a registered geometry.
GetPoseInParent(self: pydrake.geometry.SceneGraphInspector_[float], geometry_id: pydrake.geometry.GeometryId) → pydrake.math.RigidTransform_[float]

Reports the pose of the geometry G with the given id in its registered topological parent P, X_PG. That topological parent may be a frame F or another geometry. If the geometry was registered directly to F, then X_PG = X_FG.

See also

GetPoseInFrame()

Raises:RuntimeError if id does not map to a registered geometry.
GetProximityProperties(self: pydrake.geometry.SceneGraphInspector_[float], geometry_id: pydrake.geometry.GeometryId) → pydrake.geometry.ProximityProperties

Returns a pointer to the const proximity properties of the geometry with the given id.

Parameter id:
The identifier for the queried geometry.
Returns:A pointer to the properties (or nullptr if there are no such properties).
Raises:RuntimeError if id does not map to a registered geometry.
GetShape(self: pydrake.geometry.SceneGraphInspector_[float], geometry_id: pydrake.geometry.GeometryId) → pydrake.geometry.Shape

Returns the shape specified for the geometry with the given id. In order to extract the details of the shape, it should be passed through an implementation of a ShapeReifier.

GetSourceName(self: pydrake.geometry.SceneGraphInspector_[float], id: pydrake.geometry.SourceId) → str

Reports the name for the source with the given id.

Raises:RuntimeError if id does not map to a registered source.
num_frames(self: pydrake.geometry.SceneGraphInspector_[float]) → int

Reports the total number of frames registered in the scene graph (including the world frame).

num_geometries(self: pydrake.geometry.SceneGraphInspector_[float]) → int

Reports the total number of geometries in the scene graph.

num_sources(self: pydrake.geometry.SceneGraphInspector_[float]) → int

Reports the number of registered sources – whether they have registered frames/geometries or not. This will always be at least 1; the SceneGraph itself counts as a source.

NumAnchoredGeometries(self: pydrake.geometry.SceneGraphInspector_[float]) → int

Reports the total number of anchored geometries. This should provide the same answer as calling NumGeometriesForFrame() with the world frame id.

NumDynamicGeometries(self: pydrake.geometry.SceneGraphInspector_[float]) → int

Reports the total number of dynamic geometries in the scene graph.

NumGeometriesWithRole(self: pydrake.geometry.SceneGraphInspector_[float], role: pydrake.geometry.Role) → int

Reports the total number of geometries in the scene graph with the indicated role.

SourceIsRegistered(self: pydrake.geometry.SceneGraphInspector_[float], id: pydrake.geometry.SourceId) → bool

Reports True if the given id maps to a registered source.

class pydrake.geometry.Shape

The base interface for all shape specifications. It has no public constructor and cannot be instantiated directly. The Shape class has two key properties:

  • it is cloneable, and
  • it can be “reified” (see ShapeReifier).

When you add a new subclass of Shape, you must:

1. add a virtual function ImplementGeometry() for the new shape in ShapeReifier that invokes the ThrowUnsupportedGeometry method, and add to the test for it in shape_specification_test.cc. 2. implement ImplementGeometry in derived ShapeReifiers to continue support if desired, otherwise ensure unimplemented functions are not hidden in new derivations of ShapeReifier with using, for example, using ShapeReifier::ImplementGeometry. Existing subclasses should already have this.

Otherwise, you might get a runtime error. We do not have an automatic way to enforce them at compile time.

__init__

Initialize self. See help(type(self)) for accurate signature.

Clone(self: pydrake.geometry.Shape) → pydrake.geometry.Shape
pydrake.geometry.SignedDistancePair

alias of pydrake.geometry.SignedDistancePair_[float]

template pydrake.geometry.SignedDistancePair_

Instantiations: SignedDistancePair_[float], SignedDistancePair_[AutoDiffXd]

class SignedDistancePair_[float]

The data for reporting the signed distance between two geometries, A and B. It provides the id’s of the two geometries, the witness points Ca and Cb on the surfaces of A and B, the signed distance, and nhat_BA_W a direction of fastest increasing distance (always unit length and always point outward from B’s surface).

  • When A and B are separated, distance > 0.
  • When A and B are touching or penetrating, distance <= 0.
  • By definition, nhat_AB_W must be in the opposite direction of nhat_BA_W.
  • (p_WCa - p_Wcb) = distance · nhat_BA_W.
  • In some cases, nhat_BA_W is not unique, and is_nhat_BA_W_unique is false.

Warning

For two geometries that are just touching (i.e., distance = 0), the underlying code can guarantee a correct value for nhat_BA_W only when one geometry is a sphere, and the other geometry is a sphere, a box, or a cylinder. Otherwise, the underlying code is not in place yet to guarantee a correct value for nhat_BA_W when surfaces are just touching, and the vector will be populated by NaN values.

Template parameter T:
The underlying scalar type. Must be a valid Eigen scalar.
__init__(self: pydrake.geometry.SignedDistancePair_[float], **kwargs) → None

Constructor

Parameter a:
The id of the first geometry (A).
Parameter b:
The id of the second geometry (B).
Parameter p_ACa_in:
The witness point on geometry A’s surface, in A’s frame.
Parameter p_BCb_in:
The witness point on geometry B’s surface, in B’s frame.
Parameter dist:
The signed distance between p_A and p_B.
Parameter nhat_BA_W_in:
A direction of fastest increasing distance.
Parameter is_nhat_BA_W_unique_in:
True if nhat_BA_W is unique.
Precondition:
nhat_BA_W_in is unit-length.
distance

The signed distance between p_ACa and p_BCb.

id_A

The id of the first geometry in the pair.

id_B

The id of the second geometry in the pair.

is_nhat_BA_W_unique
nhat_BA_W

A direction of fastest increasing distance.

p_ACa

The witness point on geometry A’s surface, expressed in A’s frame.

p_BCb

The witness point on geometry B’s surface, expressed in B’s frame.

class pydrake.geometry.SignedDistancePair_[AutoDiffXd]

The data for reporting the signed distance between two geometries, A and B. It provides the id’s of the two geometries, the witness points Ca and Cb on the surfaces of A and B, the signed distance, and nhat_BA_W a direction of fastest increasing distance (always unit length and always point outward from B’s surface).

  • When A and B are separated, distance > 0.
  • When A and B are touching or penetrating, distance <= 0.
  • By definition, nhat_AB_W must be in the opposite direction of nhat_BA_W.
  • (p_WCa - p_Wcb) = distance · nhat_BA_W.
  • In some cases, nhat_BA_W is not unique, and is_nhat_BA_W_unique is false.

Warning

For two geometries that are just touching (i.e., distance = 0), the underlying code can guarantee a correct value for nhat_BA_W only when one geometry is a sphere, and the other geometry is a sphere, a box, or a cylinder. Otherwise, the underlying code is not in place yet to guarantee a correct value for nhat_BA_W when surfaces are just touching, and the vector will be populated by NaN values.

Template parameter T:
The underlying scalar type. Must be a valid Eigen scalar.
__init__(self: pydrake.geometry.SignedDistancePair_[AutoDiffXd], **kwargs) → None

Constructor

Parameter a:
The id of the first geometry (A).
Parameter b:
The id of the second geometry (B).
Parameter p_ACa_in:
The witness point on geometry A’s surface, in A’s frame.
Parameter p_BCb_in:
The witness point on geometry B’s surface, in B’s frame.
Parameter dist:
The signed distance between p_A and p_B.
Parameter nhat_BA_W_in:
A direction of fastest increasing distance.
Parameter is_nhat_BA_W_unique_in:
True if nhat_BA_W is unique.
Precondition:
nhat_BA_W_in is unit-length.
distance

The signed distance between p_ACa and p_BCb.

id_A

The id of the first geometry in the pair.

id_B

The id of the second geometry in the pair.

is_nhat_BA_W_unique
nhat_BA_W

A direction of fastest increasing distance.

p_ACa

The witness point on geometry A’s surface, expressed in A’s frame.

p_BCb

The witness point on geometry B’s surface, expressed in B’s frame.

class pydrake.geometry.SignedDistancePair_[float]

The data for reporting the signed distance between two geometries, A and B. It provides the id’s of the two geometries, the witness points Ca and Cb on the surfaces of A and B, the signed distance, and nhat_BA_W a direction of fastest increasing distance (always unit length and always point outward from B’s surface).

  • When A and B are separated, distance > 0.
  • When A and B are touching or penetrating, distance <= 0.
  • By definition, nhat_AB_W must be in the opposite direction of nhat_BA_W.
  • (p_WCa - p_Wcb) = distance · nhat_BA_W.
  • In some cases, nhat_BA_W is not unique, and is_nhat_BA_W_unique is false.

Warning

For two geometries that are just touching (i.e., distance = 0), the underlying code can guarantee a correct value for nhat_BA_W only when one geometry is a sphere, and the other geometry is a sphere, a box, or a cylinder. Otherwise, the underlying code is not in place yet to guarantee a correct value for nhat_BA_W when surfaces are just touching, and the vector will be populated by NaN values.

Template parameter T:
The underlying scalar type. Must be a valid Eigen scalar.
__init__(self: pydrake.geometry.SignedDistancePair_[float], **kwargs) → None

Constructor

Parameter a:
The id of the first geometry (A).
Parameter b:
The id of the second geometry (B).
Parameter p_ACa_in:
The witness point on geometry A’s surface, in A’s frame.
Parameter p_BCb_in:
The witness point on geometry B’s surface, in B’s frame.
Parameter dist:
The signed distance between p_A and p_B.
Parameter nhat_BA_W_in:
A direction of fastest increasing distance.
Parameter is_nhat_BA_W_unique_in:
True if nhat_BA_W is unique.
Precondition:
nhat_BA_W_in is unit-length.
distance

The signed distance between p_ACa and p_BCb.

id_A

The id of the first geometry in the pair.

id_B

The id of the second geometry in the pair.

is_nhat_BA_W_unique
nhat_BA_W

A direction of fastest increasing distance.

p_ACa

The witness point on geometry A’s surface, expressed in A’s frame.

p_BCb

The witness point on geometry B’s surface, expressed in B’s frame.

pydrake.geometry.SignedDistanceToPoint

alias of pydrake.geometry.SignedDistanceToPoint_[float]

template pydrake.geometry.SignedDistanceToPoint_

Instantiations: SignedDistanceToPoint_[float], SignedDistanceToPoint_[AutoDiffXd]

class SignedDistanceToPoint_[float]

The data for reporting the signed distance from a query point to a geometry. Reports the result of a signed distance query between a query point Q and geometry G. This includes G’s id, the signed distance, the nearest point N on the surface of G, and the gradient of the signed distance with respect to the position of Q. Generally, the gradient of the signed distance function is not defined everywhere. The value reported in this struct depends on the query function returning it. Refer to the query function’s documentation for what value it will report for otherwise undefined gradient values.

Template parameter T:
The underlying scalar type. Must be a valid Eigen scalar.
__init__(self: pydrake.geometry.SignedDistanceToPoint_[float], **kwargs) → None

Constructs SignedDistanceToPoint struct from calculated results.

Parameter id_G_in:
The id of the geometry G to which we measure distance from the query point Q.
Parameter p_GN_in:
The position of the nearest point N on G’s surface from the query point Q, expressed in G’s frame.
Parameter distance_in:
The signed distance from the query point Q to the nearest point N on the surface of geometry G. It is positive if Q is outside G. It is negative if Q is inside G. It is zero if Q is on the boundary of G.
Parameter grad_W_in:
The gradient vector of the distance function with respect to the query point Q, expressed in world frame W.
Parameter is_grad_W_unique_in:
True if grad_W is unique, false otherwise.

Note

grad_W is not well defined everywhere. For example, when computing the distance from a point to a sphere, and the point coincides with the center of the sphere, grad_W is not well defined (as it can be computed as p_GQ / |p_GQ|, but the denominator is 0). When grad_W is not well defined, and we instantiate SignedDistanceToPoint<T> with T being an AutoDiffScalar (like AutoDiffXd), the gradient of the query result is not well defined either, so the user should use the gradient in p_GN, distance and grad_W with caution.

Precondition:
grad_W_in must not contain NaN.
distance

The signed distance from the query point Q to the nearest point N on the surface of geometry G. It is positive if Q is outside G. It is negative if Q is inside G. It is zero if Q is on the boundary of G.

grad_W

The gradient vector of the distance function with respect to the query point Q, expressed in world frame W.

id_G

The id of the geometry G to which we measure distance from the query point Q.

p_GN

The position of the nearest point N on G’s surface from the query point Q, expressed in G’s frame.

class pydrake.geometry.SignedDistanceToPoint_[AutoDiffXd]

The data for reporting the signed distance from a query point to a geometry. Reports the result of a signed distance query between a query point Q and geometry G. This includes G’s id, the signed distance, the nearest point N on the surface of G, and the gradient of the signed distance with respect to the position of Q. Generally, the gradient of the signed distance function is not defined everywhere. The value reported in this struct depends on the query function returning it. Refer to the query function’s documentation for what value it will report for otherwise undefined gradient values.

Template parameter T:
The underlying scalar type. Must be a valid Eigen scalar.
__init__(self: pydrake.geometry.SignedDistanceToPoint_[AutoDiffXd], **kwargs) → None

Constructs SignedDistanceToPoint struct from calculated results.

Parameter id_G_in:
The id of the geometry G to which we measure distance from the query point Q.
Parameter p_GN_in:
The position of the nearest point N on G’s surface from the query point Q, expressed in G’s frame.
Parameter distance_in:
The signed distance from the query point Q to the nearest point N on the surface of geometry G. It is positive if Q is outside G. It is negative if Q is inside G. It is zero if Q is on the boundary of G.
Parameter grad_W_in:
The gradient vector of the distance function with respect to the query point Q, expressed in world frame W.
Parameter is_grad_W_unique_in:
True if grad_W is unique, false otherwise.

Note

grad_W is not well defined everywhere. For example, when computing the distance from a point to a sphere, and the point coincides with the center of the sphere, grad_W is not well defined (as it can be computed as p_GQ / |p_GQ|, but the denominator is 0). When grad_W is not well defined, and we instantiate SignedDistanceToPoint<T> with T being an AutoDiffScalar (like AutoDiffXd), the gradient of the query result is not well defined either, so the user should use the gradient in p_GN, distance and grad_W with caution.

Precondition:
grad_W_in must not contain NaN.
distance

The signed distance from the query point Q to the nearest point N on the surface of geometry G. It is positive if Q is outside G. It is negative if Q is inside G. It is zero if Q is on the boundary of G.

grad_W

The gradient vector of the distance function with respect to the query point Q, expressed in world frame W.

id_G

The id of the geometry G to which we measure distance from the query point Q.

p_GN

The position of the nearest point N on G’s surface from the query point Q, expressed in G’s frame.

class pydrake.geometry.SignedDistanceToPoint_[float]

The data for reporting the signed distance from a query point to a geometry. Reports the result of a signed distance query between a query point Q and geometry G. This includes G’s id, the signed distance, the nearest point N on the surface of G, and the gradient of the signed distance with respect to the position of Q. Generally, the gradient of the signed distance function is not defined everywhere. The value reported in this struct depends on the query function returning it. Refer to the query function’s documentation for what value it will report for otherwise undefined gradient values.

Template parameter T:
The underlying scalar type. Must be a valid Eigen scalar.
__init__(self: pydrake.geometry.SignedDistanceToPoint_[float], **kwargs) → None

Constructs SignedDistanceToPoint struct from calculated results.

Parameter id_G_in:
The id of the geometry G to which we measure distance from the query point Q.
Parameter p_GN_in:
The position of the nearest point N on G’s surface from the query point Q, expressed in G’s frame.
Parameter distance_in:
The signed distance from the query point Q to the nearest point N on the surface of geometry G. It is positive if Q is outside G. It is negative if Q is inside G. It is zero if Q is on the boundary of G.
Parameter grad_W_in:
The gradient vector of the distance function with respect to the query point Q, expressed in world frame W.
Parameter is_grad_W_unique_in:
True if grad_W is unique, false otherwise.

Note

grad_W is not well defined everywhere. For example, when computing the distance from a point to a sphere, and the point coincides with the center of the sphere, grad_W is not well defined (as it can be computed as p_GQ / |p_GQ|, but the denominator is 0). When grad_W is not well defined, and we instantiate SignedDistanceToPoint<T> with T being an AutoDiffScalar (like AutoDiffXd), the gradient of the query result is not well defined either, so the user should use the gradient in p_GN, distance and grad_W with caution.

Precondition:
grad_W_in must not contain NaN.
distance

The signed distance from the query point Q to the nearest point N on the surface of geometry G. It is positive if Q is outside G. It is negative if Q is inside G. It is zero if Q is on the boundary of G.

grad_W

The gradient vector of the distance function with respect to the query point Q, expressed in world frame W.

id_G

The id of the geometry G to which we measure distance from the query point Q.

p_GN

The position of the nearest point N on G’s surface from the query point Q, expressed in G’s frame.

class pydrake.geometry.SourceId

Type used to identify geometry sources in SceneGraph.

__init__

Initialize self. See help(type(self)) for accurate signature.

static get_new_id() → pydrake.geometry.SourceId

Generates a new identifier for this id type. This new identifier will be different from all previous identifiers created. This method does not make any guarantees about the values of ids from successive invocations. This method is guaranteed to be thread safe.

get_value(self: pydrake.geometry.SourceId) → int

Extracts the underlying representation from the identifier. This is considered invalid for invalid ids and is strictly enforced in Debug builds.

is_valid(self: pydrake.geometry.SourceId) → bool

Reports if the id is valid.

class pydrake.geometry.Sphere

Bases: pydrake.geometry.Shape

Definition of sphere. It is centered in its canonical frame with the given radius.

__init__(self: pydrake.geometry.Sphere, radius: float) → None

Constructs a sphere with the given radius.

Raises:
  • RuntimeError if radius is negative. Note that a zero radius is
  • is considered valid.
radius(self: pydrake.geometry.Sphere) → float
pydrake.geometry.SurfaceMesh

alias of pydrake.geometry.SurfaceMesh_[float]

template pydrake.geometry.SurfaceMesh_

Instantiations: SurfaceMesh_[float], SurfaceMesh_[AutoDiffXd]

class SurfaceMesh_[float]

SurfaceMesh represents a triangulated surface.

Template parameter T:
The underlying scalar type for coordinates, e.g., double or AutoDiffXd. Must be a valid Eigen scalar.
__init__(self: pydrake.geometry.SurfaceMesh_[float], faces: List[drake::geometry::SurfaceFace], vertices: List[pydrake.geometry.SurfaceVertex_[float]]) → None

Constructs a SurfaceMesh from faces and vertices.

Parameter faces:
The triangular faces.
Parameter vertices:
The vertices.
faces(self: pydrake.geometry.SurfaceMesh_[float]) → List[drake::geometry::SurfaceFace]

Returns the faces.

vertices(self: pydrake.geometry.SurfaceMesh_[float]) → List[pydrake.geometry.SurfaceVertex_[float]]

Returns the vertices.

class pydrake.geometry.SurfaceMesh_[AutoDiffXd]

SurfaceMesh represents a triangulated surface.

Template parameter T:
The underlying scalar type for coordinates, e.g., double or AutoDiffXd. Must be a valid Eigen scalar.
__init__(self: pydrake.geometry.SurfaceMesh_[AutoDiffXd], faces: List[drake::geometry::SurfaceFace], vertices: List[pydrake.geometry.SurfaceVertex_[AutoDiffXd]]) → None

Constructs a SurfaceMesh from faces and vertices.

Parameter faces:
The triangular faces.
Parameter vertices:
The vertices.
faces(self: pydrake.geometry.SurfaceMesh_[AutoDiffXd]) → List[drake::geometry::SurfaceFace]

Returns the faces.

vertices(self: pydrake.geometry.SurfaceMesh_[AutoDiffXd]) → List[pydrake.geometry.SurfaceVertex_[AutoDiffXd]]

Returns the vertices.

class pydrake.geometry.SurfaceMesh_[float]

SurfaceMesh represents a triangulated surface.

Template parameter T:
The underlying scalar type for coordinates, e.g., double or AutoDiffXd. Must be a valid Eigen scalar.
__init__(self: pydrake.geometry.SurfaceMesh_[float], faces: List[drake::geometry::SurfaceFace], vertices: List[pydrake.geometry.SurfaceVertex_[float]]) → None

Constructs a SurfaceMesh from faces and vertices.

Parameter faces:
The triangular faces.
Parameter vertices:
The vertices.
faces(self: pydrake.geometry.SurfaceMesh_[float]) → List[drake::geometry::SurfaceFace]

Returns the faces.

vertices(self: pydrake.geometry.SurfaceMesh_[float]) → List[pydrake.geometry.SurfaceVertex_[float]]

Returns the vertices.

pydrake.geometry.SurfaceVertex

alias of pydrake.geometry.SurfaceVertex_[float]

template pydrake.geometry.SurfaceVertex_

Instantiations: SurfaceVertex_[float], SurfaceVertex_[AutoDiffXd]

class SurfaceVertex_[float]

SurfaceVertex represents a vertex in SurfaceMesh.

Template parameter T:
The underlying scalar type for coordinates, e.g., double or AutoDiffXd. Must be a valid Eigen scalar.
__init__(self: pydrake.geometry.SurfaceVertex_[float], r_MV: numpy.ndarray[numpy.float64[3, 1]]) → None

Constructs SurfaceVertex.

Parameter r_MV:
displacement vector from the origin of M’s frame to this vertex, expressed in M’s frame.
r_MV(self: pydrake.geometry.SurfaceVertex_[float]) → numpy.ndarray[numpy.float64[3, 1]]

Returns the displacement vector from the origin of M’s frame to this vertex, expressed in M’s frame.

class pydrake.geometry.SurfaceVertex_[AutoDiffXd]

SurfaceVertex represents a vertex in SurfaceMesh.

Template parameter T:
The underlying scalar type for coordinates, e.g., double or AutoDiffXd. Must be a valid Eigen scalar.
__init__(self: pydrake.geometry.SurfaceVertex_[AutoDiffXd], r_MV: numpy.ndarray[object[3, 1]]) → None

Constructs SurfaceVertex.

Parameter r_MV:
displacement vector from the origin of M’s frame to this vertex, expressed in M’s frame.
r_MV(self: pydrake.geometry.SurfaceVertex_[AutoDiffXd]) → numpy.ndarray[object[3, 1]]

Returns the displacement vector from the origin of M’s frame to this vertex, expressed in M’s frame.

class pydrake.geometry.SurfaceVertex_[float]

SurfaceVertex represents a vertex in SurfaceMesh.

Template parameter T:
The underlying scalar type for coordinates, e.g., double or AutoDiffXd. Must be a valid Eigen scalar.
__init__(self: pydrake.geometry.SurfaceVertex_[float], r_MV: numpy.ndarray[numpy.float64[3, 1]]) → None

Constructs SurfaceVertex.

Parameter r_MV:
displacement vector from the origin of M’s frame to this vertex, expressed in M’s frame.
r_MV(self: pydrake.geometry.SurfaceVertex_[float]) → numpy.ndarray[numpy.float64[3, 1]]

Returns the displacement vector from the origin of M’s frame to this vertex, expressed in M’s frame.