dair_pll.geometry

Collision geometry representation for multibody systems.

Each type of collision geometry is modeled as a class inheriting from the CollisionGeometry abstract type. Different types of inheriting geometries will need to resolve collisions in unique ways, but one interface is always expected: a list of scalars to track during training.

Many collision geometries, such as boxes and cylinders, fall into the class of bounded (compact) convex shapes. A general interface is defined in the abstract BoundedConvexCollisionGeometry type, which returns a set of witness points given support hyperplane directions. One implementation is the SparseVertexConvexCollisionGeometry type, which finds these points via brute force optimization over a short list of vertices.

All collision geometries implemented here mirror a Drake Shape object. A general purpose converter is implemented in PydrakeToCollisionGeometryFactory.

class dair_pll.geometry.CollisionGeometry(*args, **kwargs)

Bases: ABC, Module

Abstract interface for collision geometries.

Collision geometries have heterogeneous implementation depending on the type of shape. This class mainly enforces the implementation of bookkeeping interfaces.

When two collision geometries are evaluated for collision with GeometryCollider, their ordering is constrained via a total order on collision geometry types, enforced with an overload of the > operator.

Initializes internal Module state, shared by both nn.Module and ScriptModule.

abstract scalars()

Describes object via Tensorboard scalars.

Any namespace for the object (e.g. “object_5”) is assumed to be added by external code before adding to Tensorboard, so the names of the scalars can be natural descriptions of the geometry’s parameters.

Examples

A cylinder might be represented with the following output:

{'radius': 5.2, 'height': 4.1}

Return type:

Dict[str, float]

Returns:

A dictionary of named parameters describing the geometry.

class dair_pll.geometry.Plane(*args, **kwargs)

Bases: CollisionGeometry

Half-space/plane collision geometry.

Plane geometries are assumed to be the plane z=0 in local (i.e. “body-axes” coordinates). Any tilted/raised/lowered half spaces are expected to be derived by placing the z=0 plane in a rigidly-transformed frame.

Initializes internal Module state, shared by both nn.Module and ScriptModule.

scalars()

As the plane is fixed to be z=0, there are no parameters.

Return type:

Dict[str, float]

class dair_pll.geometry.BoundedConvexCollisionGeometry(*args, **kwargs)

Bases: CollisionGeometry

Interface for compact-convex collision geometries.

Such shapes have a representation via a “support function” h(d), which takes in a hyperplane direction and returns how far the shape S extends in that direction, i.e.:

h(d) = max_{s \in S} s \cdot d.

This representation can be leveraged to derive “witness points” – i.e. the closest point(s) between the BoundedConvexCollisionGeometry and another convex shape, such as another BoundedConvexCollisionGeometry or a Plane.

Initializes internal Module state, shared by both nn.Module and ScriptModule.

abstract support_points(directions)

Returns a set of witness points representing contact with another shape off in the direction(s) directions.

This method will return a set of points S' \subset S such that:

argmax_{s \in S} s \cdot directions \subset convexHull(S').

In theory, returning exactly the argmax set would be sufficient. However,

Parameters:

directions (Tensor) – (*, 3) batch of unit-length directions.

Return type:

Tensor

Returns:

(*, N, 3) sets of corresponding witness points of cardinality N.

class dair_pll.geometry.SparseVertexConvexCollisionGeometry(n_query)

Bases: BoundedConvexCollisionGeometry

Partial implementation of BoundedConvexCollisionGeometry when witness points are guaranteed to be contained in a small set of vertices.

An obvious subtype is any sort of Polytope, such as a Box. A less obvious subtype are shapes in which a direction-dependent set of vertices can be easily calculated. See Cylinder, for instance.

Inits SparseVertexConvexCollisionGeometry with prescribed query interface.

Parameters:

n_query (int) – number of vertices to return in witness point set.

support_points(directions)

Implements BoundedConvexCollisionGeometry.support_points() via brute force optimization over the witness vertex set.

Specifically, if S_v is the vertex set, this function returns n_query elements s of S_v for which s \cdot directions is highest. This set is not guaranteed to be sorted.

Given the prescribed behavior of BoundedConvexCollisionGeometry.support_points(), an implicit assumption of this implementation is that the convex hull of the top n_query points in S_v contains argmax_{s \in S} s \cdot directions.

Parameters:

directions (Tensor) – (*, 3) batch of directions.

Return type:

Tensor

Returns:

(*, n_query, 3) sets of corresponding witness points.

abstract get_vertices(directions)

Returns sparse witness point set as collection of vertices.

Specifically, given search directions, returns a set of points S_v for which:

argmax_{s \in S} s \cdot directions \subset convexHull(S_v).

Parameters:

directions (Tensor) – (*, 3) batch of unit-length directions.

Return type:

Tensor

Returns:

(*, N, 3) witness point convex hull vertices.

class dair_pll.geometry.Polygon(vertices, n_query=4)

Bases: SparseVertexConvexCollisionGeometry

Concrete implementation of a convex polytope.

Implemented via SparseVertexConvexCollisionGeometry as a static set of vertices, where models the underlying shape as all convex combinations of the vertices.

Inits Polygon object with initial vertex set.

Parameters:

• vertices (Tensor) – (N, 3) static vertex set.

• n_query (int) – number of vertices to return in witness point set.

vertices: Parameter

get_vertices(directions)

Return batched view of static vertex set

Return type:

Tensor

scalars()

Return one scalar for each vertex index.

Return type:

Dict[str, float]

class dair_pll.geometry.DeepSupportConvex(vertices, n_query=4, depth=2, width=256, perturbation=0.4)

Bases: SparseVertexConvexCollisionGeometry

Deep support function convex shape.

Any convex shape \(S\) can be equivalently represented via its support function \(f(d)\), which returns the extent to which the object extends in the \(d\) direction:

\[f(d) = \max_{s \in S} s \cdot d.\]

Given a direction, the set of points that form the \(\arg\max\) in \(f(d)\) is exactly the convex subgradient \(\partial_d f(d)\).

Furthermore, for every convex shape, \(f(d)\) is convex and positively homogeneous, and every convex and positively homogeneous \(f(d)\) is the support function of some convex shape.

This collision geometry type implements the support function directly as a convex and positively homogeneous neural network ( HomogeneousICNN).

Inits DeepSupportConvex object with initial vertex set.

When calculating a sparse vertex set with get_vertices(), supplements the support direction with nearby directions randomly.

Parameters:

• vertices (Tensor) – (N, 3) initial vertex set.

• n_query (int) – Number of vertices to return in witness point set.

• depth (int) – Depth of support function network.

• width (int) – Width of support function network.

• perturbation (float) – support direction sampling parameter.

fcl_geometry: BVHModel

fcl mesh collision geometry representation.

network: HomogeneousICNN

Support function representation as a neural net.

perturbations: Tensor

Perturbed support directions, which aid mesh-plane contact stability.

get_vertices(directions)

Return batched view of support points of interest.

Given a direction \(d\), this function finds the support point of the object in that direction, calculated via envelope

Parameters:

directions (Tensor) – (*, 3) batch of support directions sample.

Return type:

Tensor

Returns:

(*, n_query, 3) sampled support points.

train(mode=True)

Override training-mode setter from torch.

Sets a static fcl mesh geometry for the entirety of evaluation time, as the underlying support function is not changing.

Parameters:

mode (bool) – True for training, False for evaluation.

Return type:

DeepSupportConvex

Returns:

self.

get_fcl_geometry()

Retrieves fcl mesh collision geometry representation.

If evaluation mode is set, retrieves precalculated version.

Return type:

BVHModel

Returns:

fcl bounding volume hierarchy for mesh.

scalars()

no scalars!

Return type:

Dict[str, float]

class dair_pll.geometry.Box(half_lengths, n_query)

Bases: SparseVertexConvexCollisionGeometry

Implementation of cuboid geometry as a sparse vertex convex hull.

To prevent learning negative box lengths, the learned parameters are stored as length_params, and the box’s half lengths can be computed as their absolute value. The desired half lengths can be accessed via get_half_lengths().

Inits Box object with initial size.

Parameters:

• half_lengths (Tensor) – (3,) half-length dimensions of box on x, y, and z axes.

• n_query (int) – number of vertices to return in witness point set.

length_params: Parameter

unit_vertices: Tensor

get_half_lengths()

From the stored length_params, compute the half lengths of the box as its absolute value.

Return type:

Tensor

get_vertices(directions)

Returns view of cuboid’s static vertex set.

Return type:

Tensor

scalars()

Returns each axis’s full length as a scalar.

Return type:

Dict[str, float]

class dair_pll.geometry.Sphere(radius)

Bases: BoundedConvexCollisionGeometry

Implements sphere geometry via its support function.

It is trivial to calculate the witness point for a sphere contact as simply the product of the sphere’s radius and the support direction.

To prevent learning a negative radius, the learned parameter is stored as length_param, and the sphere’s radius can be computed as its absolute value. The desired radius can be accessed via get_radius().

Initializes internal Module state, shared by both nn.Module and ScriptModule.

length_param: Parameter

get_radius()

From the stored length_param, compute the radius of the sphere as its absolute value.

Return type:

Tensor

support_points(directions)

Implements BoundedConvexCollisionGeometry.support_points() via analytic expression:

argmax_{s \in S} s \cdot directions = directions * radius.

Parameters:

directions (Tensor) – (*, 3) batch of directions.

Return type:

Tensor

Returns:

(*, 1, 3) corresponding witness point sets of cardinality 1.

scalars()

Logs radius as a scalar.

Return type:

Dict[str, float]

class dair_pll.geometry.PydrakeToCollisionGeometryFactory

Bases: object

Utility class for converting Drake Shape instances to CollisionGeometry instances.

static convert(drake_shape)

Converts abstract pydrake.geometry.shape to CollisionGeometry.

Parameters:

drake_shape (Shape) – drake shape type to convert.

Return type:

CollisionGeometry

Returns:

Collision geometry representation of shape.

Raises:

TypeError – When provided object is not a supported Drake shape type.

static convert_box(drake_box)

Converts pydrake.geometry.Box to Box.

Return type:

Box

static convert_plane()

Converts pydrake.geometry.HalfSpace to Plane.

Return type:

Plane

static convert_mesh(drake_mesh)

Converts pydrake.geometry.Mesh to Polygon.

Return type:

DeepSupportConvex

class dair_pll.geometry.GeometryCollider

Bases: object

Utility class for colliding two CollisionGeometry instances.

static collide(geometry_a, geometry_b, R_AB, p_AoBo_A)

Collides two collision geometries.

Takes in the two geometries as well as a relative transform between them. This function is a thin shell for other static methods of GeometryCollider where the given geometries are guaranteed to have specific types.

Parameters:

• geometry_a (CollisionGeometry) – first collision geometry

• geometry_b (CollisionGeometry) – second collision geometry, with type ordering not geometry_A > geometry_B.

• R_AB (Tensor) – (*,3,3) rotation between geometry frames

• p_AoBo_A (Tensor) – (*, 3) offset of geometry frame origins

Return type:

Tuple[Tensor, Tensor, Tensor, Tensor]

Returns:

(*, N) batch of witness point pair distances (*, N, 3, 3) contact frame C rotation in A, R_AC, where the z axis of C is contained in the normal cone of body A at contact point Ac and is parallel (or antiparallel) to AcBc. (*, N, 3) witness points Ac on A, p_AoAc_A (*, N, 3) witness points Bc on B, p_BoBc_B

static collide_plane_convex(geometry_b, R_AB, p_AoBo_A)

Implementation of GeometryCollider.collide() when geometry_a is a Plane and geometry_b is a BoundedConvexCollisionGeometry.

Return type:

Tuple[Tensor, Tensor, Tensor, Tensor]

static collide_mesh_mesh(geometry_a, geometry_b, R_AB, p_AoBo_A)

Implementation of GeometryCollider.collide() when both geometries are DeepSupportConvexes.

Return type:

Tuple[Tensor, Tensor, Tensor, Tensor]