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.

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.

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.

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

Type used to identify geometry frames in SceneGraph.

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<Isometry3<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 Isometry3<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 (Isometry3, 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<Isometry3<Scalar>> | double FramePoseVector | FrameKinematicsVector<Isometry3<Scalar>> | AutoDiffXd FramePoseVector | FrameKinematicsVector<Isometry3<Scalar>> | Expression

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.common.eigen_geometry.Isometry3_[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.common.eigen_geometry.Isometry3_[float]

Returns the value associated with the given id.

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

Type used to identify geometry instances in SceneGraph.

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

class pydrake.geometry.Mesh

Bases: pydrake.geometry.Shape

Limited support for meshes. Meshes declared as such will not serve in proximity queries or rendering queries. However, they will be propagated to drake_visualizer. The mesh is dispatched to drake visualizer via the filename. The mesh is not parsed/loaded by Drake.

pydrake.geometry.PenetrationAsPointPair

alias of pydrake.geometry.PenetrationAsPointPair_[float]

template pydrake.geometry.PenetrationAsPointPair_

Instantiations: PenetrationAsPointPair_[float], PenetrationAsPointPair_[AutoDiffXd]

class PenetrationAsPointPair_[float]
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.

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.

Template parameter T:
The scalar type. Must be a valid Eigen scalar.

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

  • double
  • AutoDiffXd

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

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

Computes the penetrations across all pairs of geometries in the world. Only reports results for penetrating geometries; if two geometries are separated, there will be no result for that pair. Pairs of anchored geometry are also not reported. The penetration between two geometries is characterized as a point pair (see PenetrationAsPointPair). This method is affected by collision filtering.

Scalar support

This method only provides double-valued penetration results.

Returns:A vector populated with all detected penetrations characterized as point pairs.
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.

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

class pydrake.geometry.Role

General enumeration for indicating geometry role.

Members:

kUnassigned :

kProximity :

kIllustration :

kPerception :

kIllustration = Role.kIllustration
kPerception = Role.kPerception
kProximity = Role.kProximity
kUnassigned = Role.kUnassigned
name

(self – handle) -> str

pydrake.geometry.SceneGraph

alias of pydrake.geometry.SceneGraph_[float]

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

@system{SceneGraph, @input_port{source_pose{0}} @input_port{…} @input_port{source_pose{N-1}}, @output_port{lcm_visualization} @output_port{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.

Template parameter T:
The scalar type. Must be a valid Eigen scalar.

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

  • double
  • AutoDiffXd

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

RegisterSource(self: pydrake.geometry.SceneGraph_[float], name: str = '') → pydrake.geometry.SourceId

Registers a new 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.
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.
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 pure virtual function ImplementGeometry() for the new shape in ShapeReifier. 2. define ImplementGeometry() for the new shape in the subclasses of ShapeReifier. 3. modify CopyShapeOrThrow() of ProximityEngine to support the new shape and add an instance of the new shape to the CopySemantics test in proximity_engine_test.cc. 4. test the new shape in the class BoxPenetrationTest of proximity_engine_test.cc

Otherwise, you might get a runtime error. We do not have an automatic way to enforce them at compile time.

pydrake.geometry.SignedDistancePair

alias of pydrake.geometry.SignedDistancePair_[float]

template pydrake.geometry.SignedDistancePair_

Instantiations: SignedDistancePair_[float], SignedDistancePair_[AutoDiffXd]

class SignedDistancePair_[float]
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]
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.

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.