pydrake.geometry.optimization

Local bindings for drake::geometry::optimization

class pydrake.geometry.optimization.AffineBall

Bases: pydrake.geometry.optimization.ConvexSet

Implements an ellipsoidal convex set represented as an affine scaling of the unit ball {Bu + center | |u|₂ ≤ 1}. B must be a square matrix.

Compare this with an alternative parametrization of the ellipsoid: {x | (x-center)ᵀAᵀA(x-center) ≤ 1}, which utilizes a quadratic form. The two representations are related by B = A⁻¹ if A and B are invertible.

The quadratic form parametrization is implemented in Hyperellipsoid. It can represent unbounded sets, but not sets along a lower-dimensional affine subspace. The AffineBall parametrization can represent sets along a lower-dimensional affine subspace, but not unbounded sets.

An AffineBall can never be empty – it always contains its center. This includes the zero-dimensional case.

__init__(*args, **kwargs)

Overloaded function.

  1. __init__(self: pydrake.geometry.optimization.AffineBall) -> None

Constructs a default (zero-dimensional, nonempty) set.

  1. __init__(self: pydrake.geometry.optimization.AffineBall, B: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], center: numpy.ndarray[numpy.float64[m, 1]]) -> None

Constructs the ellipsoid from a transformation matrix B and translation center. B describes the linear transformation that is applied to the unit ball in order to produce the ellipsoid, and center describes the translation of the center of the ellipsoid from the origin.

Precondition:

B.rows() == B.cols().

Precondition:

B.cols() == center.size().

  1. __init__(self: pydrake.geometry.optimization.AffineBall, ellipsoid: pydrake.geometry.optimization.Hyperellipsoid) -> None

Constructs an AffineBall from a Hyperellipsoid.

Precondition:

ellipsoid.IsBounded().

B(self: pydrake.geometry.optimization.AffineBall) numpy.ndarray[numpy.float64[m, n]]

Returns the affine transformation matrix B.

center(self: pydrake.geometry.optimization.AffineBall) numpy.ndarray[numpy.float64[m, 1]]

Returns the center of the ellipsoid.

Clone(self: pydrake.geometry.optimization.AffineBall) pydrake.geometry.optimization.ConvexSet
static MakeAxisAligned(radius: numpy.ndarray[numpy.float64[m, 1]], center: numpy.ndarray[numpy.float64[m, 1]]) pydrake.geometry.optimization.AffineBall

Constructs an axis-aligned AffineBall with the implicit form (x₀-c₀)²/r₀² + (x₁-c₁)²/r₁² + … + (x_N - c_N)²/r_N² ≤ 1, where c is shorthand for center and r is shorthand for radius.

Precondition:

radius.size() == center.size().

Precondition:

radius[i] >= 0, for all i.

static MakeHypersphere(radius: float, center: numpy.ndarray[numpy.float64[m, 1]]) pydrake.geometry.optimization.AffineBall

Constructs a hypersphere with radius and center.

Precondition:

radius >= 0.

static MakeUnitBall(dim: int) pydrake.geometry.optimization.AffineBall

Constructs the L₂-norm unit ball in dim dimensions, {x | |x|₂ <= 1 }.

Precondition:

dim >= 0.

static MinimumVolumeCircumscribedEllipsoid(points: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], rank_tol: float = 1e-06) pydrake.geometry.optimization.AffineBall

Constructs the minimum-volume ellipsoid which contains all of the given points. This is commonly referred to as the outer Löwner-John ellipsoid.

If all of the points lie along a proper affine subspace, this method instead computes the minimum-k-volume ellipsoid, where k is the affine dimension of the convex hull of points.

Parameter points:

is a d-by-n matrix, where d is the ambient dimension and each column represents one point.

Parameter rank_tol:

the tolerance used to detect if the data lies in an affine subspace. The affine ball is computed in the subspace spanned by the left singular vectors of the data matrix whose associated singular values are larger than rank_tol * max_singular_value. The default is 1e-6 to be compatible with common solver tolerances.

Raises
  • RuntimeError if the MathematicalProgram fails to solve. This can

  • happen due to numerical issues caused by rank_tol being set

  • too low.

  • RuntimeError if points includes NaNs or infinite values.

Precondition:

points.rows() >= 1.

Precondition:

points.cols() >= 1.

class pydrake.geometry.optimization.AffineSubspace

Bases: pydrake.geometry.optimization.ConvexSet

An affine subspace (also known as a “flat”, a “linear variety”, or a “linear manifold”) is a vector subspace of some Euclidean space, potentially translated so as to not pass through the origin. Examples include points, lines, and planes (not necessarily through the origin).

An affine subspace is described by a basis of its corresponding vector subspace, plus a translation. This description is not unique as any point in the affine subspace can be used as a translation, and any basis of the corresponding vector subspace is valid.

An affine subspace can never be empty, because a vector subspace can never be empty. Thus, the translation will always be contained in the flat. An affine subspace is bounded if it is a point, which is when the basis has zero columns.

__init__(*args, **kwargs)

Overloaded function.

  1. __init__(self: pydrake.geometry.optimization.AffineSubspace) -> None

Constructs a default (zero-dimensional, nonempty) affine subspace.

  1. __init__(self: pydrake.geometry.optimization.AffineSubspace, basis: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], translation: numpy.ndarray[numpy.float64[m, 1]]) -> None

Constructs the affine subspace from an n-by-m matrix describing the basis, where n is the ambient dimension, and m is the dimension of the subspace, and from an n-dimensional vector describing the translation. The set is {x | x = translation + basis*y, y ∈ Rᵐ} The columns must be linearly independent.

Precondition:

basis.rows() == translation.size().

  1. __init__(self: pydrake.geometry.optimization.AffineSubspace, set: pydrake.geometry.optimization.ConvexSet, tol: float = 1e-12) -> None

Constructs an affine subspace as the affine hull of another convex set. The generic approach is to find a feasible point in the set, and then iteratively compute feasible vectors until we have a basis that spans the set. If you pass in a convex set whose points are matrix-valued (e.g. a Spectrahedron), then the affine subspace will work over a flattened representation of those coordinates. (So a Spectrahedron with n-by-n matrices will output an AffineSubspace with ambient dimension (n * (n+1)) / 2.)

tol sets the numerical precision of the computation. For each dimension, a pair of feasible points are constructed, so as to maximize the displacement in that dimension. If their displacement along that dimension is larger than tol, then the vector connecting the points is added as a basis vector.

Raises
  • RuntimeError if set is empty.

  • RuntimeError if tol < 0.

For several subclasses of ConvexSet, there is a closed-form computation (or more efficient numerical computation) that is preferred. - AffineBall: Can be computed via a rank-revealing decomposition; tol is used as the numerical tolerance for the rank of the matrix. Pass std::nullopt for tol to use Eigen’s automatic tolerance computation. - AffineSubspace: Equivalent to the copy-constructor; tol is ignored. - CartesianProduct: Can compute the affine hull of each factor individually; tol is propagated to the constituent calls. (This is not done if the Cartesian product has an associated affine transformation.) - Hyperellipsoid: Always equal to the whole ambient space; tol is ignored. - Hyperrectangle: Can be computed in closed-form; tol has the same meaning as in the generic affine hull computation. - Point: Can be computed in closed-form; tol is ignored. This also encompasses sets which are obviously a singleton point, as determined via MaybeGetPoint. - VPolytope: Can be computed via a singular value decomposition; tol is used as the numerical tolerance for the rank of the matrix. Pass std::nullopt for tol to use Eigen’s automatic tolerance computation.

AffineDimension(self: pydrake.geometry.optimization.AffineSubspace) int

Returns the affine dimension of this set. For an affine subspace, this is simply the number of columns in the basis_ matrix. A point will have affine dimension zero.

basis(self: pydrake.geometry.optimization.AffineSubspace) numpy.ndarray[numpy.float64[m, n]]

Returns the basis in an n-by-m matrix, where n is the ambient dimension, and m is the number of vectors in the basis.

Clone(self: pydrake.geometry.optimization.AffineSubspace) pydrake.geometry.optimization.ConvexSet
ContainedIn(self: pydrake.geometry.optimization.AffineSubspace, other: pydrake.geometry.optimization.AffineSubspace, tol: float = 1e-15) bool

Returns True if this AffineSubspace is contained in other. This is computed by checking if translation() is in other and then checking if each basis vector is in the span of the basis of other. The latter step requires finding a least-squares solution, so a nonzero tolerance (tol) is almost always necessary. (You may have to adjust the default tolerance depending on the dimension of your space and the scale of your basis vectors.)

IsNearlyEqualTo(self: pydrake.geometry.optimization.AffineSubspace, other: pydrake.geometry.optimization.AffineSubspace, tol: float = 1e-15) bool

Returns True if this AffineSubspace is contained in other. This is computed by checking if translation() is in other and then checking if each basis vector is in the span of the basis of other. The latter step requires finding a least-squares solution, so a nonzero tolerance (tol) is almost always necessary. (You may have to adjust the default tolerance depending on the dimension of your space and the scale of your basis vectors.)

OrthogonalComplementBasis(self: pydrake.geometry.optimization.AffineSubspace) numpy.ndarray[numpy.float64[m, n]]

Returns an orthonormal basis of the vector subspace which is orthogonal to this AffineSubspace.

ToGlobalCoordinates(self: pydrake.geometry.optimization.AffineSubspace, y: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous]) numpy.ndarray[numpy.float64[m, n]]

Given a point y in the basis of the AffineSubspace, with the zero point at translation_, returns the coordinates of y in the standard basis of the ambient space. If the AffineSubspace is a point, it has an empty basis, so the only possible local coordinates are also empty (and should be passed in as a length-zero vector). Each column of the input should be a vector in the affine subspace, represented in its local coordinates, and the corresponding column of the output will be its representation in the coordinate system of the ambient space.

Precondition:

y.rows() == AffineDimension()

ToLocalCoordinates(self: pydrake.geometry.optimization.AffineSubspace, x: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous]) numpy.ndarray[numpy.float64[m, n]]

Given a point x in the standard basis of the ambient space, returns the coordinates of x in the basis of the AffineSubspace, with the zero point at translation_. The component of x that is orthogonal to the AffineSubspace (if it exists) is discarded, so ToGlobalCoordinates(ToLocalCoordinates(x)) is equivalent to Project(x). Note that if the AffineSubspace is a point, the basis is empty, so the local coordinates will also be empty (and returned as a length-zero vector). Each column of the input should be a vector in the ambient space, and the corresponding column of the output will be its representation in the local coordinates of the affine subspace.

Precondition:

x.rows() == ambient_dimension()

translation(self: pydrake.geometry.optimization.AffineSubspace) numpy.ndarray[numpy.float64[m, 1]]

Returns the translation as a length n vector.

pydrake.geometry.optimization.CalcPairwiseIntersections(*args, **kwargs)

Overloaded function.

  1. CalcPairwiseIntersections(convex_sets_A: list[pydrake.geometry.optimization.ConvexSet], convex_sets_B: list[pydrake.geometry.optimization.ConvexSet], continuous_revolute_joints: list[int], preprocess_bbox: bool = True) -> list[tuple[int, int, numpy.ndarray[numpy.float64[m, 1]]]]

Computes the pairwise intersections between two lists of convex sets, returning a list of edges. Each edge is a tuple in the form [index_A, index_B, offset_A_to_B], where index_A is the index of the list in convex_sets_A, index_B is the index of the list in convex_sets_B, and offset_A_to_B is is the translation to applied to all the points in the index_A’th set in convex_sets_A to align them with the index_B’th set in convex_sets_B. This translation may only have non-zero entries along the dimensions corresponding to continuous_revolute_joints. All non-zero entries are integer multiples of 2π as the translation of the sets still represents the same configurations for the indices in continuous_revolute_joints.

Parameter convex_sets_A:

is a vector of convex sets. Pairwise intersections will be computed between convex_sets_A and convex_sets_B.

Parameter convex_sets_B:

is the other vector of convex sets.

Parameter continuous_revolute_joints:

is a list of joint indices corresponding to continuous revolute joints.

Parameter preprocess_bbox:

is a flag for whether the function should precompute axis-aligned bounding boxes (AABBs) for every set. This can speed up the pairwise intersection checks, by determining some sets to be disjoint without needing to solve an optimization problem. However, it does require some overhead to compute those bounding boxes.

Raises
  • if continuous_revolute_joints has repeated entries, or if any

  • entry is outside the interval [0, ambient_dimension), where

  • ambient_dimension is the ambient dimension of the convex sets in

  • convex_sets_A` and convex_sets_B

  • if convex_sets_A or convex_sets_B are empty. */

  • (Deprecated.)

Deprecated:

Instead use ComputePairwiseIntersections, with return type std::pair<std::vector<std::pair<int, int>>, std::vector<Eigen::VectorXd>>. This will be removed from Drake on or after 2024-12-01.

  1. CalcPairwiseIntersections(convex_sets_A: list[pydrake.geometry.optimization.ConvexSet], convex_sets_B: list[pydrake.geometry.optimization.ConvexSet], continuous_revolute_joints: list[int], bboxes_A: list[pydrake.geometry.optimization.Hyperrectangle], bboxes_B: list[pydrake.geometry.optimization.Hyperrectangle]) -> list[tuple[int, int, numpy.ndarray[numpy.float64[m, 1]]]]

Overload of CalcPairwiseIntersections allowing the user to supply axis- aligned bounding boxes if they’re known a priori, to save on computation time.

Parameter bboxes_A:

is a vector of Hyperrectangles, allowing the user to manually pass in the AABBs of each set in convex_sets_A to avoid recomputation.

Parameter bboxes_B:

serves the same role to convex_sets_B as bboxes_A does to convex_sets_A.

Warning

The function does not check that the entries of bboxes_A are indeed the AABBs corresponding to the sets in convex_sets_A (and likewise for bboxes_B).

Raises
  • if convex_sets_A.size() != bboxes_A.size()

  • if convex_sets_B.size() != bboxes_B.size()

  • if not all entries of convex_sets_A, convex_sets_B,

  • bboxes_A`, and bboxes_B have the same ambient dimension. *

  • (Deprecated.)

Deprecated:

Instead use ComputePairwiseIntersections, with return type std::pair<std::vector<std::pair<int, int>>, std::vector<Eigen::VectorXd>>. This will be removed from Drake on or after 2024-12-01.

  1. CalcPairwiseIntersections(convex_sets: list[pydrake.geometry.optimization.ConvexSet], continuous_revolute_joints: list[int], preprocess_bbox: bool = True) -> list[tuple[int, int, numpy.ndarray[numpy.float64[m, 1]]]]

Convenience overload to compute pairwise intersections within a list of convex sets. Equivalent to calling CalcPairwiseIntersections(convex_sets, convex_sets, continuous_revolute_joints).

Parameter convex_sets:

is a vector of convex sets. Pairwise intersections will be computed within convex_sets.

Parameter continuous_revolute_joints:

is a list of joint indices corresponding to continuous revolute joints.

Parameter preprocess_bbox:

is a flag for whether the function should precompute axis-aligned bounding boxes for every set. This can speed up the pairwise intersection checks, by determining some sets to be disjoint without needing to solve an optimization problem.

Raises
  • if continuous_revolute_joints has repeated entries, or if any

  • entry is outside the interval [0, ambient_dimension), where

  • ambient_dimension is the ambient dimension of the convex sets in

  • convex_sets`

  • if convex_sets is empty. */ (Deprecated.)

Deprecated:

Instead use ComputePairwiseIntersections, with return type std::pair<std::vector<std::pair<int, int>>, std::vector<Eigen::VectorXd>>. This will be removed from Drake on or after 2024-12-01.

  1. CalcPairwiseIntersections(convex_sets: list[pydrake.geometry.optimization.ConvexSet], continuous_revolute_joints: list[int], bboxes: list[pydrake.geometry.optimization.Hyperrectangle] = []) -> list[tuple[int, int, numpy.ndarray[numpy.float64[m, 1]]]]

Overload of CalcPairwiseIntersections allowing the user to supply axis- aligned bounding boxes if they’re known a priori, to save on computation time.

Parameter bboxes:

is a vector of Hyperrectangles, allowing the user to manually pass in the AABBs of each set in convex_sets to avoid recomputation.

Warning

The function does not check that the entries are indeed the AABBs corresponding to the sets in convex_sets.

Raises
  • if convex_sets.size() != bboxes.size()

  • if not all entries of convex_sets and bboxes have the same

  • ambient dimension. */ (Deprecated.)

Deprecated:

Instead use ComputePairwiseIntersections, with return type std::pair<std::vector<std::pair<int, int>>, std::vector<Eigen::VectorXd>>. This will be removed from Drake on or after 2024-12-01.

class pydrake.geometry.optimization.CartesianProduct

Bases: pydrake.geometry.optimization.ConvexSet

The Cartesian product of convex sets is a convex set: S = X₁ × X₂ × ⋯ × Xₙ = {(x₁, x₂, …, xₙ) | x₁ ∈ X₁, x₂ ∈ X₂, …, xₙ ∈ Xₙ}.

This class also supports a generalization of this concept in which the coordinates are transformed by the linear map, {x | y = Ax + b, y ∈ Y₁ × Y₂ × ⋯ × Yₙ}, with the default values set to the identity map. This concept is required for reasoning about cylinders in arbitrary poses as cartesian products, and more generally for describing any affine transform of a CartesianProduct.

Special behavior for IsEmpty: If there are no sets in the product, returns nonempty by convention. See: https://en.wikipedia.org/wiki/Empty_product#Nullary_Cartesian_product Otherwise, if any set in the cartesian product is empty, the whole product is empty.

__init__(*args, **kwargs)

Overloaded function.

  1. __init__(self: pydrake.geometry.optimization.CartesianProduct) -> None

Constructs a default (zero-dimensional, nonempty) set.

  1. __init__(self: pydrake.geometry.optimization.CartesianProduct, sets: list[pydrake.geometry.optimization.ConvexSet]) -> None

Constructs the product from a vector of convex sets.

  1. __init__(self: pydrake.geometry.optimization.CartesianProduct, setA: pydrake.geometry.optimization.ConvexSet, setB: pydrake.geometry.optimization.ConvexSet) -> None

Constructs the product from a pair of convex sets.

  1. __init__(self: pydrake.geometry.optimization.CartesianProduct, sets: list[pydrake.geometry.optimization.ConvexSet], A: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], b: numpy.ndarray[numpy.float64[m, 1]]) -> None

Constructs the product of convex sets in the transformed coordinates: {x | y = Ax + b, y ∈ Y₁ × Y₂ × ⋯ × Yₙ}.

Raises

RuntimeError when A is not full column rank.

  1. __init__(self: pydrake.geometry.optimization.CartesianProduct, query_object: pydrake.geometry.QueryObject, geometry_id: pydrake.geometry.GeometryId, reference_frame: Optional[pydrake.geometry.FrameId] = None) -> None

Constructs a CartesianProduct from a SceneGraph geometry and pose in the reference_frame frame, obtained via the QueryObject. If reference_frame frame is std::nullopt, then it will be expressed in the world frame.

Although any geometry that can be used as a ConvexSet could also be a (trivial) CartesianProduct, we restrict this constructor to handling Cylinder geometry, which constructs the (non-trivial) Cartesian product of a HyperEllipsoid and an HPolyhedron. Most other SceneGraph geometry types are supported by at least one of the ConvexSet class constructors.

Raises

RuntimeError if geometry_id does not correspond to a Cylinder.

A(self: pydrake.geometry.optimization.CartesianProduct) Optional[numpy.ndarray[numpy.float64[m, n]]]

Returns a copy of the matrix A if it has been set, or nullopt otherwise.

b(self: pydrake.geometry.optimization.CartesianProduct) Optional[numpy.ndarray[numpy.float64[m, 1]]]

Returns a copy of the vector b if it has been set, or nullopt otherwise.

factor(self: pydrake.geometry.optimization.CartesianProduct, index: int) pydrake.geometry.optimization.ConvexSet

Returns a reference to the ConvexSet defining the index factor in the product.

num_factors(self: pydrake.geometry.optimization.CartesianProduct) int

The number of factors (or sets) used in the product.

pydrake.geometry.optimization.CheckIfSatisfiesConvexityRadius(convex_set: pydrake.geometry.optimization.ConvexSet, continuous_revolute_joints: list[int]) bool

Given a convex set, and a list of indices corresponding to continuous revolute joints, checks whether or not the set satisfies the convexity radius. See §6.5.3 of “A Panoramic View of Riemannian Geometry”, Marcel Berger for a general definition of convexity radius. When dealing with continuous revolute joints, respecting the convexity radius entails that each convex set has a width of stricty less than π along each dimension corresponding to a continuous revolute joint.

Raises
  • RuntimeError if continuous_revolute_joints has repeated entries,

  • or if any entry is outside the interval [0,

  • convex_set.ambient_dimension())

class pydrake.geometry.optimization.CIrisCollisionGeometry

This class contains the necessary information about the collision geometry used in C-IRIS. Most notably it transcribes the geometric condition that the collision geometry is on one side of the plane to mathematical constraints. For the detailed algorithm please refer to the paper Certified Polyhedral Decompositions of Collision-Free Configuration Space by Hongkai Dai*, Alexandre Amice*, Peter Werner, Annan Zhang and Russ Tedrake.

__init__(*args, **kwargs)
body_index(self: pydrake.geometry.optimization.CIrisCollisionGeometry) pydrake.multibody.tree.BodyIndex
geometry(self: pydrake.geometry.optimization.CIrisCollisionGeometry) pydrake.geometry.Shape
id(self: pydrake.geometry.optimization.CIrisCollisionGeometry) pydrake.geometry.GeometryId
num_rationals(self: pydrake.geometry.optimization.CIrisCollisionGeometry) int

Returns the number of rationals in the condition “this geometry is on one side of the plane.”

type(self: pydrake.geometry.optimization.CIrisCollisionGeometry) pydrake.geometry.optimization.CIrisGeometryType
X_BG(self: pydrake.geometry.optimization.CIrisCollisionGeometry) pydrake.math.RigidTransform
class pydrake.geometry.optimization.CIrisGeometryType

The supported type of geometries in C-IRIS.

Members:

kPolytope :

kSphere :

kCylinder :

kCapsule :

__init__(self: pydrake.geometry.optimization.CIrisGeometryType, value: int) None
kCapsule = <CIrisGeometryType.kCapsule: 3>
kCylinder = <CIrisGeometryType.kCylinder: 2>
kPolytope = <CIrisGeometryType.kPolytope: 1>
kSphere = <CIrisGeometryType.kSphere: 0>
property name
property value
pydrake.geometry.optimization.ComputePairwiseIntersections(*args, **kwargs)

Overloaded function.

  1. ComputePairwiseIntersections(convex_sets_A: list[pydrake.geometry.optimization.ConvexSet], convex_sets_B: list[pydrake.geometry.optimization.ConvexSet], continuous_revolute_joints: list[int], preprocess_bbox: bool = True) -> tuple[list[tuple[int, int]], list[numpy.ndarray[numpy.float64[m, 1]]]]

Computes the pairwise intersections between two lists of convex sets, returning a list of edges, and a list of their corresponding offsets. Each edge is a tuple in the form [index_A, index_B], where index_A is the index of the set in convex_sets_A and index_B is the index of the set in convex_sets_B. The corresponding entry in the list of offsets (i.e., the entry at the same index) is the translation that is applied to all the points in the index_A’th set in convex_sets_A to align them with the index_B’th set in convex_sets_B. This translation may only have non-zero entries along the dimensions corresponding to continuous_revolute_joints. All non-zero entries are integer multiples of 2π as the translation of the sets still represents the same configurations for the indices in continuous_revolute_joints.

Parameter convex_sets_A:

is a vector of convex sets. Pairwise intersections will be computed between convex_sets_A and convex_sets_B.

Parameter convex_sets_B:

is the other vector of convex sets.

Parameter continuous_revolute_joints:

is a list of joint indices corresponding to continuous revolute joints.

Parameter preprocess_bbox:

is a flag for whether the function should precompute axis-aligned bounding boxes (AABBs) for every set. This can speed up the pairwise intersection checks, by determining some sets to be disjoint without needing to solve an optimization problem. However, it does require some overhead to compute those bounding boxes.

Raises
  • if continuous_revolute_joints has repeated entries, or if any

  • entry is outside the interval [0, ambient_dimension), where

  • ambient_dimension is the ambient dimension of the convex sets in

  • convex_sets_A` and convex_sets_B

  • if convex_sets_A or convex_sets_B are empty.

  1. ComputePairwiseIntersections(convex_sets_A: list[pydrake.geometry.optimization.ConvexSet], convex_sets_B: list[pydrake.geometry.optimization.ConvexSet], continuous_revolute_joints: list[int], bboxes_A: list[pydrake.geometry.optimization.Hyperrectangle], bboxes_B: list[pydrake.geometry.optimization.Hyperrectangle]) -> tuple[list[tuple[int, int]], list[numpy.ndarray[numpy.float64[m, 1]]]]

Overload of ComputePairwiseIntersections allowing the user to supply axis- aligned bounding boxes if they’re known a priori, to save on computation time.

Parameter bboxes_A:

is a vector of Hyperrectangles, allowing the user to manually pass in the AABBs of each set in convex_sets_A to avoid recomputation.

Parameter bboxes_B:

serves the same role to convex_sets_B as bboxes_A does to convex_sets_A.

Warning

The function does not check that the entries of bboxes_A are indeed the AABBs corresponding to the sets in convex_sets_A (and likewise for bboxes_B).

Raises
  • if convex_sets_A.size() != bboxes_A.size()

  • if convex_sets_B.size() != bboxes_B.size()

  • if not all entries of convex_sets_A, convex_sets_B,

  • bboxes_A`, and bboxes_B have the same ambient dimension

  1. ComputePairwiseIntersections(convex_sets: list[pydrake.geometry.optimization.ConvexSet], continuous_revolute_joints: list[int], preprocess_bbox: bool = True) -> tuple[list[tuple[int, int]], list[numpy.ndarray[numpy.float64[m, 1]]]]

Convenience overload to compute pairwise intersections within a list of convex sets. Equivalent to calling ComputePairwiseIntersections(convex_sets, convex_sets, continuous_revolute_joints).

Parameter convex_sets:

is a vector of convex sets. Pairwise intersections will be computed within convex_sets.

Parameter continuous_revolute_joints:

is a list of joint indices corresponding to continuous revolute joints.

Parameter preprocess_bbox:

is a flag for whether the function should precompute axis-aligned bounding boxes for every set. This can speed up the pairwise intersection checks, by determining some sets to be disjoint without needing to solve an optimization problem.

Raises
  • if continuous_revolute_joints has repeated entries, or if any

  • entry is outside the interval [0, ambient_dimension), where

  • ambient_dimension is the ambient dimension of the convex sets in

  • convex_sets`

  • if convex_sets is empty.

  1. ComputePairwiseIntersections(convex_sets: list[pydrake.geometry.optimization.ConvexSet], continuous_revolute_joints: list[int], bboxes: list[pydrake.geometry.optimization.Hyperrectangle] = []) -> tuple[list[tuple[int, int]], list[numpy.ndarray[numpy.float64[m, 1]]]]

Overload of ComputePairwiseIntersections allowing the user to supply axis- aligned bounding boxes if they’re known a priori, to save on computation time.

Parameter bboxes:

is a vector of Hyperrectangles, allowing the user to manually pass in the AABBs of each set in convex_sets to avoid recomputation.

Warning

The function does not check that the entries are indeed the AABBs corresponding to the sets in convex_sets.

Raises
  • if convex_sets.size() != bboxes.size()

  • if not all entries of convex_sets and bboxes have the same

  • ambient dimension.

class pydrake.geometry.optimization.ConvexHull

Bases: pydrake.geometry.optimization.ConvexSet

Implements the convex hull of a set of convex sets. The convex hull of multiple sets is defined as the smallest convex set that contains all the sets. Given non-empty convex sets {X₁, X₂, …, Xₙ}, the convex hull is the set of all convex combinations of points in the sets, i.e. {∑ᵢ λᵢ xᵢ | xᵢ ∈ Xᵢ, λᵢ ≥ 0, ∑ᵢ λᵢ = 1}.

__init__(self: pydrake.geometry.optimization.ConvexHull, sets: list[pydrake.geometry.optimization.ConvexSet], remove_empty_sets: bool = True) None

Constructs the convex hull from a vector of convex sets.

Parameter sets:

A vector of convex sets that define the convex hull.

Parameter remove_empty_sets:

If true, the constructor will check if any of the sets are empty and will not consider them. If false, the constructor will not check if any of the sets are empty.

Warning

If remove_empty_sets is set to false, but some of the sets are in fact empty, then unexpected and incorrect results may occur. Only set this flag to false if you are sure that your sets are non-empty and performance in the constructor is critical.

element(self: pydrake.geometry.optimization.ConvexHull, index: int) pydrake.geometry.optimization.ConvexSet

Returns a reference to the convex set at the given index (including empty sets).

empty_sets_removed(self: pydrake.geometry.optimization.ConvexHull) bool

Returns true if this was constructed with remove_empty_sets=true.

num_elements(self: pydrake.geometry.optimization.ConvexHull) int

Returns the number of convex sets defining the convex hull (including empty sets).

participating_sets(self: pydrake.geometry.optimization.ConvexHull) object

Returns the participating convex sets.

sets(self: pydrake.geometry.optimization.ConvexHull) object

Returns the participating convex sets.

class pydrake.geometry.optimization.ConvexSet

Abstract base class for defining a convex set.

__init__(*args, **kwargs)
AddPointInNonnegativeScalingConstraints(*args, **kwargs)

Overloaded function.

  1. AddPointInNonnegativeScalingConstraints(self: pydrake.geometry.optimization.ConvexSet, prog: pydrake.solvers.MathematicalProgram, x: numpy.ndarray[object[m, 1]], t: pydrake.symbolic.Variable) -> list[pydrake.solvers.Binding[Constraint]]

Let S be this convex set. When S is bounded, this method adds the convex constraints to imply

Click to expand C++ code...
x ∈ t S,
t ≥ 0,

where x is a point in ℜⁿ (with n the ambient_dimension) and t is a scalar.

When S is unbounded, then the behavior is almost identical, except when t=0. In this case, the constraints imply t ≥ 0, x ∈ t S ⊕ rec(S), where rec(S) is the recession cone of S (the asymptotic directions in which S is not bounded) and ⊕ is the Minkowski sum. For t > 0, this is equivalent to x ∈ t S, but for t = 0, we have only x ∈ rec(S).

Raises

RuntimeError if ambient_dimension() == 0

  1. AddPointInNonnegativeScalingConstraints(self: pydrake.geometry.optimization.ConvexSet, prog: pydrake.solvers.MathematicalProgram, A: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], b: numpy.ndarray[numpy.float64[m, 1]], c: numpy.ndarray[numpy.float64[m, 1]], d: float, x: numpy.ndarray[object[m, 1]], t: numpy.ndarray[object[m, 1]]) -> list[pydrake.solvers.Binding[Constraint]]

Let S be this convex set. When S is bounded, this method adds the convex constraints to imply

Click to expand C++ code...
A * x + b ∈ (c' * t + d) S,
c' * t + d ≥ 0,

where A is an n-by-m matrix (with n the ambient_dimension), b is a vector of size n, c is a vector of size p, x is a point in ℜᵐ, and t is a point in ℜᵖ.

When S is unbounded, then the behavior is almost identical, except when c’ * t+d=0. In this case, the constraints imply

Click to expand C++ code...
A * x + b ∈ (c' * t + d) S ⊕ rec(S),
c' * t + d ≥ 0,

where rec(S) is the recession cone of S (the asymptotic directions in which S is not bounded) and ⊕ is the Minkowski sum. For c’ * t + d > 0, this is equivalent to A * x + b ∈ (c’ * t + d) S, but for c’ * t + d = 0, we have only A * x + b ∈ rec(S).

Raises

RuntimeError if ambient_dimension() == 0

AddPointInSetConstraints(self: pydrake.geometry.optimization.ConvexSet, prog: pydrake.solvers.MathematicalProgram, vars: numpy.ndarray[object[m, 1]]) tuple[numpy.ndarray[object[m, 1]], list[pydrake.solvers.Binding[Constraint]]]

Adds a constraint to an existing MathematicalProgram enforcing that the point defined by vars is inside the set.

Returns

(new_vars, new_constraints) Some of the derived class will add new decision variables to enforce this constraint, we return all the newly added decision variables as new_vars. The meaning of these new decision variables differs in each subclass. If no new variables are added, then we return an empty Eigen vector. Also we return all the newly added constraints to prog through this function.

Raises

RuntimeError if ambient_dimension() == 0

ambient_dimension(self: pydrake.geometry.optimization.ConvexSet) int

Returns the dimension of the vector space in which the elements of this set are evaluated. Contrast this with the affine dimension: the dimension of the smallest affine subset of the ambient space that contains our set. For example, if we define a set using A*x = b, where A has linearly independent rows, then the ambient dimension is the dimension of x, but the affine dimension of the set is ambient_dimension() - rank(A).

CalcVolume(self: pydrake.geometry.optimization.ConvexSet) float

Computes the exact volume for the convex set.

Note

Not every convex set can report an exact volume. In that case, use CalcVolumeViaSampling() instead.

Raises
  • RuntimeError if has_exact_volume() returns False.

  • if ambient_dimension() == 0.

CalcVolumeViaSampling(self: pydrake.geometry.optimization.ConvexSet, generator: pydrake.common.RandomGenerator, desired_rel_accuracy: float = 0.01, max_num_samples: int = 10000.0) pydrake.geometry.optimization.SampledVolume

Calculates an estimate of the volume of the convex set using sampling and performing Monte Carlo integration.

Note

this method is intended to be used for low to moderate dimensions (d<15). For larger dimensions, a telescopic product approach has yet to be implemented. See, e.g., https://proceedings.mlr.press/v151/chevallier22a/chevallier22a.pdf

Parameter generator:

a random number generator.

Parameter desired_rel_accuracy:

the desired relative accuracy of the volume estimate in the sense that the estimated volume is likely to be within the interval defined by (1±2*desired_rel_accuracy)*true_volume with probability of at least* 0.95 according to the Law of Large Numbers. https://people.math.umass.edu/~lr7q/ps_files/teaching/math456/Chapter6.pdf The computation will terminate when the relative error is less than rel_accuracy or when the maximum number of samples is reached.

Parameter max_num_samples:

the maximum number of samples to use.

Precondition:

desired_rel_accuracy is in the range [0,1].

Returns

a pair the estimated volume of the set and an upper bound for the relative accuracy

Raises
  • if ambient_dimension() == 0.

  • if the minimum axis-aligned bounding box of the set cannot be

  • computed.

Clone(self: pydrake.geometry.optimization.ConvexSet) pydrake.geometry.optimization.ConvexSet

Creates a unique deep copy of this set.

IntersectsWith(self: pydrake.geometry.optimization.ConvexSet, other: pydrake.geometry.optimization.ConvexSet) bool

Returns true iff the intersection between this and other is non-empty.

Raises
  • RuntimeError if the ambient dimension of other is not the same

  • as that of this.

IsBounded(self: pydrake.geometry.optimization.ConvexSet, parallelism: pydrake.common.Parallelism = Parallelism(num_threads=1)) bool

Returns true iff the set is bounded, e.g., there exists an element-wise finite lower and upper bound for the set. Note: for some derived classes, this check is trivial, but for others it can require solving a number of (typically small) optimization problems. Each derived class documents the cost of its boundedness test and whether it honors the request for parallelism. (Derived classes which do not have a specialized check will, by default, honor parallelism requests.) Note that the overhead of multithreading may lead to slower runtimes for simple, low-dimensional sets, but can enable major speedups for more challenging problems.

Parameter parallelism:

requests the number of cores to use when solving mathematical programs to check boundedness, subject to whether a particular derived class honors parallelism.

IsEmpty(self: pydrake.geometry.optimization.ConvexSet) bool

Returns true iff the set is empty. Note: for some derived classes, this check is trivial, but for others, it can require solving a (typically small) optimization problem. Check the derived class documentation for any notes. Zero-dimensional sets must be handled specially. There are two possible sets in a zero-dimensional space – the empty set, and the whole set (which is simply the “zero vector space”, {0}.) For more details, see: https://en.wikipedia.org/wiki/Examples_of_vector_spaces#Trivial_or_zero_vector_space Zero-dimensional sets are considered to be nonempty by default. Sets which can be zero-dimensional and empty must handle this behavior in their derived implementation of DoIsEmpty. An example of such a subclass is VPolytope.

MaybeGetFeasiblePoint(self: pydrake.geometry.optimization.ConvexSet) Optional[numpy.ndarray[numpy.float64[m, 1]]]

Returns a feasible point within this convex set if it is nonempty, and nullopt otherwise.

MaybeGetPoint(self: pydrake.geometry.optimization.ConvexSet) Optional[numpy.ndarray[numpy.float64[m, 1]]]

If this set trivially contains exactly one point, returns the value of that point. Otherwise, returns nullopt. By “trivially”, we mean that representation of the set structurally maps to a single point; if checking for point-ness would require solving an optimization program, returns nullopt. In other words, this is a relatively cheap function to call.

PointInSet(self: pydrake.geometry.optimization.ConvexSet, x: numpy.ndarray[numpy.float64[m, 1]], tol: float = 1e-08) bool

Returns true iff the point x is contained in the set. If the ambient dimension is zero, then if the set is nonempty, the point is trivially in the set, and if the set is empty, the point is trivially not in the set.

Projection(self: pydrake.geometry.optimization.ConvexSet, points: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous]) Optional[tuple[list[float], numpy.ndarray[numpy.float64[m, n]]]]

Computes in the L₂ norm the distance and the nearest point in this convex set to every column of points. If this set is empty, we return nullopt.

Precondition:

points.rows() == ambient_dimension().

Raises

if the internal convex optimization solver fails.

ToShapeWithPose(self: pydrake.geometry.optimization.ConvexSet) tuple[pydrake.geometry.Shape, pydrake.math.RigidTransform]

Constructs a Shape and a pose of the set in the world frame for use in the SceneGraph geometry ecosystem.

Raises
  • RuntimeError if ambient_dimension() != 3 or if the functionality

  • for a particular set has not been implemented yet.

class pydrake.geometry.optimization.CspaceFreePolytope

Bases: pydrake.geometry.optimization.CspaceFreePolytopeBase

This class tries to find large convex polytopes in the tangential-configuration space, such that all configurations in the convex polytopes is collision free. By tangential-configuration space, we mean the revolute joint angle θ is replaced by t = tan(θ/2). We refer to the algorithm as C-IRIS. For more details, refer to the paper

Certified Polyhedral Decomposition of Collision-Free Configuration Space by Hongkai Dai*, Alexandre Amice*, Peter Werner, Annan Zhang and Russ Tedrake.

A conference version is published at

Finding and Optimizing Certified, Collision-Free Regions in Configuration Space for Robot Manipulators by Alexandre Amice*, Hongkai Dai*, Peter Werner, Annan Zhang and Russ Tedrake.

__init__(self: pydrake.geometry.optimization.CspaceFreePolytope, plant: drake::multibody::MultibodyPlant<double>, scene_graph: pydrake.geometry.SceneGraph, plane_order: pydrake.geometry.optimization.SeparatingPlaneOrder, q_star: numpy.ndarray[numpy.float64[m, 1]], options: pydrake.geometry.optimization.CspaceFreePolytopeBase.Options = Options(with_cross_y=False)) None
Parameter plant:

The plant for which we compute the C-space free polytopes. It must outlive this CspaceFreePolytope object.

Parameter scene_graph:

The scene graph that has been connected with plant. It must outlive this CspaceFreePolytope object.

Parameter plane_order:

The order of the polynomials in the plane to separate a pair of collision geometries.

Parameter q_star:

Refer to RationalForwardKinematics for its meaning.

Note

CspaceFreePolytope knows nothing about contexts. The plant and scene_graph must be fully configured before instantiating this class.

class BilinearAlternationOptions

Options for bilinear alternation.

__init__(self: pydrake.geometry.optimization.CspaceFreePolytope.BilinearAlternationOptions) None
property convergence_tol

When the change of the cost function between two consecutive iterations in bilinear alternation is no larger than this number, stop the bilinear alternation. Must be non-negative.

property ellipsoid_scaling

After finding the maximal inscribed ellipsoid in C-space polytope {s | C*s<=d, s_lower<=s<=s_upper}, we scale this ellipsoid by ellipsoid_scaling, and require the new C-space polytope to contain this scaled ellipsoid. ellipsoid_scaling=1 corresponds to no scaling. Must be strictly positive and no greater than 1.

property find_lagrangian_options
property find_polytope_options
property max_iter

The maximum number of bilinear alternation iterations. Must be non-negative.

BinarySearch(self: pydrake.geometry.optimization.CspaceFreePolytope, ignored_collision_pairs: set[Tuple[pydrake.geometry.GeometryId]], C: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], d: numpy.ndarray[numpy.float64[m, 1]], s_center: numpy.ndarray[numpy.float64[m, 1]], options: pydrake.geometry.optimization.CspaceFreePolytope.BinarySearchOptions) Optional[pydrake.geometry.optimization.CspaceFreePolytope.SearchResult]

Binary search on d such that the C-space polytope {s | C*s<=d, s_lower<=s<=s_upper} is collision free. We scale the polytope {s | C*s<=d_init} about its center s_center and search the scaling factor.

Precondition:

s_center is in the polytope {s | C*s<=d_init, s_lower<=s<=s_upper}

class BinarySearchOptions

Options for binary search.

__init__(self: pydrake.geometry.optimization.CspaceFreePolytope.BinarySearchOptions) None
property convergence_tol
property find_lagrangian_options
property max_iter
property scale_max
property scale_min
class EllipsoidMarginCost

The cost used when fixing the Lagrangian multiplier and search for C and d in the C-space polytope {s | C*s <=d, s_lower<=s<=s_upper}. We denote δᵢ as the margin between the i’th face C.row(i)<=d(i) to the inscribed ellipsoid.

Members:

kSum

kGeometricMean

__init__(self: pydrake.geometry.optimization.CspaceFreePolytope.EllipsoidMarginCost, value: int) None
kGeometricMean = <EllipsoidMarginCost.kGeometricMean: 1>
kSum = <EllipsoidMarginCost.kSum: 0>
property name
property value
class FindPolytopeGivenLagrangianOptions

Options for finding polytope with given Lagrangians.

__init__(self: pydrake.geometry.optimization.CspaceFreePolytope.FindPolytopeGivenLagrangianOptions) None
property backoff_scale
property ellipsoid_margin_cost
property ellipsoid_margin_epsilon
property s_inner_pts
property search_s_bounds_lagrangians
property solver_id
property solver_options
FindSeparationCertificateGivenPolytope(self: pydrake.geometry.optimization.CspaceFreePolytope, C: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], d: numpy.ndarray[numpy.float64[m, 1]], ignored_collision_pairs: set[Tuple[pydrake.geometry.GeometryId]], options: pydrake.geometry.optimization.CspaceFreePolytope.FindSeparationCertificateGivenPolytopeOptions) tuple[bool, dict[Tuple[pydrake.geometry.GeometryId], pydrake.geometry.optimization.CspaceFreePolytope.SeparationCertificateResult]]

Finds the certificates that the C-space polytope {s | C*s<=d, s_lower <= s <= s_upper} is collision free.

Parameter C:

The C-space polytope is {s | C*s<=d, s_lower<=s<=s_upper}

Parameter d:

The C-space polytope is {s | C*s<=d, s_lower<=s<=s_upper}

Parameter ignored_collision_pairs:

We will ignore the pair of geometries in ignored_collision_pairs.

Parameter certificates:

Contains the certificate we successfully found for each pair of geometries. Notice that depending on options, the program could search for the certificate for each geometry pair in parallel, and will terminate the search once it fails to find the certificate for any pair.

Returns success:

If true, then we have certified that the C-space polytope {s | C*s<=d, s_lower<=s<=s_upper} is collision free. Otherwise success=false.

class FindSeparationCertificateGivenPolytopeOptions

Bases: pydrake.geometry.optimization.FindSeparationCertificateOptions

__init__(self: pydrake.geometry.optimization.CspaceFreePolytope.FindSeparationCertificateGivenPolytopeOptions) None
property ignore_redundant_C
MakeIsGeometrySeparableProgram(self: pydrake.geometry.optimization.CspaceFreePolytope, geometry_pair: Tuple[pydrake.geometry.GeometryId], C: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], d: numpy.ndarray[numpy.float64[m, 1]]) pydrake.geometry.optimization.CspaceFreePolytope.SeparationCertificateProgram

Constructs the MathematicalProgram which searches for a separation certificate for a pair of geometries for a C-space polytope. Search for the separation certificate for a pair of geometries for a C-space polytope {s | C*s<=d, s_lower<=s<=s_upper}.

Raises
  • an error if this geometry_pair doesn't need separation

  • certificate (for example, they are on the same body)

class SearchResult

Result on searching the C-space polytope and separating planes.

__init__(self: pydrake.geometry.optimization.CspaceFreePolytope.SearchResult) None
a(self: pydrake.geometry.optimization.CspaceFreePolytope.SearchResult) dict[int, numpy.ndarray[object[3, 1]]]
b(self: pydrake.geometry.optimization.CspaceFreePolytope.SearchResult) dict[int, pydrake.symbolic.Polynomial]
C(self: pydrake.geometry.optimization.CspaceFreePolytope.SearchResult) numpy.ndarray[numpy.float64[m, n]]
certified_polytope(self: pydrake.geometry.optimization.CspaceFreePolytope.SearchResult) pydrake.geometry.optimization.HPolyhedron
d(self: pydrake.geometry.optimization.CspaceFreePolytope.SearchResult) numpy.ndarray[numpy.float64[m, 1]]
num_iter(self: pydrake.geometry.optimization.CspaceFreePolytope.SearchResult) int
SearchWithBilinearAlternation(self: pydrake.geometry.optimization.CspaceFreePolytope, ignored_collision_pairs: set[Tuple[pydrake.geometry.GeometryId]], C_init: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], d_init: numpy.ndarray[numpy.float64[m, 1]], options: pydrake.geometry.optimization.CspaceFreePolytope.BilinearAlternationOptions) list[pydrake.geometry.optimization.CspaceFreePolytope.SearchResult]

Search for a collision-free C-space polytope. {s | C*s<=d, s_lower<=s<=s_upper} through bilinear alternation. The goal is to maximize the volume the C-space polytope. Since we can’t compute the polytope volume in the closed form, we use the volume of the maximal inscribed ellipsoid as a surrogate function of the polytope volume.

Parameter ignored_collision_pairs:

The pairs of geometries that we ignore when searching for separation certificates.

Parameter C_init:

The initial value of C.

Parameter d_init:

The initial value of d.

Parameter options:

The options for the bilinear alternation.

Returns results:

Stores the certification result in each iteration of the bilinear alternation.

class SeparatingPlaneLagrangians

When searching for the separating plane, we want to certify that the numerator of a rational is non-negative in the C-space region C*s<=d, s_lower <= s <= s_upper. Hence for each of the rational we will introduce Lagrangian multipliers for the polytopic constraint d-C*s >= 0, s - s_lower >= 0, s_upper - s >= 0.

__init__(self: pydrake.geometry.optimization.CspaceFreePolytope.SeparatingPlaneLagrangians, C_rows: int, s_size: int) None
GetSolution(self: pydrake.geometry.optimization.CspaceFreePolytope.SeparatingPlaneLagrangians, result: pydrake.solvers.MathematicalProgramResult) pydrake.geometry.optimization.CspaceFreePolytope.SeparatingPlaneLagrangians

Substitutes the decision variables in each Lagrangians with its value in result, returns the substitution result.

polytope(self: pydrake.geometry.optimization.CspaceFreePolytope.SeparatingPlaneLagrangians) numpy.ndarray[object[m, 1]]
s_lower(self: pydrake.geometry.optimization.CspaceFreePolytope.SeparatingPlaneLagrangians) numpy.ndarray[object[m, 1]]
s_upper(self: pydrake.geometry.optimization.CspaceFreePolytope.SeparatingPlaneLagrangians) numpy.ndarray[object[m, 1]]
class SeparationCertificate

This struct stores the necessary information to search for the separating plane for the polytopic C-space region C*s <= d, s_lower <= s <= s_upper. We need to impose that N rationals are non-negative in this C-space polytope. The denominator of each rational is always positive hence we need to impose the N numerators are non-negative in this C-space region. We impose the condition numerator_i(s) - λ(s)ᵀ * (d - C*s) - λ_lower(s)ᵀ * (s - s_lower) -λ_upper(s)ᵀ * (s_upper - s) is sos λ(s) are sos, λ_lower(s) are sos, λ_upper(s) are sos.

__init__(*args, **kwargs)
GetSolution(self: pydrake.geometry.optimization.CspaceFreePolytope.SeparationCertificate, plane_index: int, a: numpy.ndarray[object[3, 1]], b: pydrake.symbolic.Polynomial, plane_decision_vars: numpy.ndarray[object[m, 1]], result: pydrake.solvers.MathematicalProgramResult) pydrake.geometry.optimization.CspaceFreePolytope.SeparationCertificateResult
property negative_side_rational_lagrangians
property positive_side_rational_lagrangians
class SeparationCertificateProgram

Bases: pydrake.geometry.optimization.SeparationCertificateProgramBase

__init__(self: pydrake.geometry.optimization.CspaceFreePolytope.SeparationCertificateProgram) None
property certificate
property plane_index
class SeparationCertificateResult

We certify that a pair of geometries is collision free in the C-space region {s | Cs<=d, s_lower<=s<=s_upper} by finding the separating plane and the Lagrangian multipliers. This struct contains the certificate, that the separating plane {x | aᵀx+b=0 } separates the two geometries in separating_planes()[plane_index] in the C-space polytope.

__init__(*args, **kwargs)
property a

The separating plane is { x | aᵀx+b=0 }

property b
property negative_side_rational_lagrangians
property plane_decision_var_vals
property plane_index
property positive_side_rational_lagrangians
property result
SolveSeparationCertificateProgram(self: pydrake.geometry.optimization.CspaceFreePolytope, certificate_program: pydrake.geometry.optimization.CspaceFreePolytope.SeparationCertificateProgram, options: pydrake.geometry.optimization.CspaceFreePolytope.FindSeparationCertificateGivenPolytopeOptions) Optional[pydrake.geometry.optimization.CspaceFreePolytope.SeparationCertificateResult]

Solves a SeparationCertificateProgram with the given options

Returns

result If we find the separation certificate, then result contains the separation plane and the Lagrangian polynomials; otherwise result is empty.

class pydrake.geometry.optimization.CspaceFreePolytopeBase

This virtual class is the base of CspaceFreePolytope and CspaceFreeBox. We take the common functionality between these concrete derived class to this shared parent class.

__init__(*args, **kwargs)
map_geometries_to_separating_planes(self: pydrake.geometry.optimization.CspaceFreePolytopeBase) dict[Tuple[pydrake.geometry.GeometryId], int]

separating_planes()[map_geometries_to_separating_planes.at(geometry1_id, geometry2_id)] is the separating plane that separates geometry 1 and geometry 2.

class Options

Optional argument for constructing CspaceFreePolytopeBase

__init__(self: pydrake.geometry.optimization.CspaceFreePolytopeBase.Options) None
property with_cross_y

For non-polytopic collision geometries, we will impose a matrix-sos constraint X(s) being psd, with a slack indeterminates y, such that the polynomial

Click to expand C++ code...
p(s, y) = ⌈ 1 ⌉ᵀ * X(s) * ⌈ 1 ⌉
⌊ y ⌋           ⌊ y ⌋

is positive. This p(s, y) polynomial doesn’t contain the cross term of y (namely it doesn’t have y(i)*y(j), i≠j). When we select the monomial basis for this polynomial, we can also exclude the cross term of y in the monomial basis.

To illustrate the idea, let’s consider the following toy example: if we want to certify that a(0) + a(1)*y₀ + a(2)*y₁ + a(3)*y₀² + a(4)*y₁² is positive (this polynomial doesn’t have the cross term y₀*y₁), we can write it as

Click to expand C++ code...
⌈ 1⌉ᵀ * A₀ * ⌈ 1⌉ + ⌈ 1⌉ᵀ * A₁ * ⌈ 1⌉
⌊y₀⌋         ⌊y₀⌋   ⌊y₁⌋         ⌊y₁⌋

with two small psd matrices A₀, A₁ Instead of

Click to expand C++ code...
⌈ 1⌉ᵀ * A * ⌈ 1⌉
|y₀|        |y₀|
⌊y₁⌋        ⌊y₁⌋

with one large psd matrix A. The first parameterization won’t have the cross term y₀*y₁ by construction, while the second parameterization requires imposing extra constraints on certain off-diagonal terms in A so that the cross term vanishes.

If we set with_cross_y = false, then we will use the monomial basis that doesn’t generate cross terms of y, leading to smaller size sos problems. If we set with_cross_y = true, then we will use the monomial basis that will generate cross terms of y, causing larger size sos problems, but possibly able to certify a larger C-space polytope.

separating_planes(self: pydrake.geometry.optimization.CspaceFreePolytopeBase) list[pydrake.geometry.optimization.CSpaceSeparatingPlane_[Variable]]

All the separating planes between each pair of geometries.

y_slack(self: pydrake.geometry.optimization.CspaceFreePolytopeBase) numpy.ndarray[object[3, 1]]

Get the slack variable used for non-polytopic collision geometries. Check Options class for more details.

class pydrake.geometry.optimization.CSpaceSeparatingPlane

Wraps the information that a pair of collision geometries are separated by a plane. One collision geometry is on the “positive” side of the separating plane, namely {x| aᵀx + b ≥ δ} (with δ ≥ 0}, and the other collision geometry is on the “negative” side of the separating plane, namely {x|aᵀx+b ≤ −δ}.

Template parameter T:

The type of decision_variables. T= symbolic::Variable or double.

Note

This class is templated; see CSpaceSeparatingPlane_ for the list of instantiations.

__init__(*args, **kwargs)
property a
property b
property decision_variables
property expressed_body
property negative_side_geometry
property plane_degree
property positive_side_geometry
template pydrake.geometry.optimization.CSpaceSeparatingPlane_

Instantiations: CSpaceSeparatingPlane_[float], CSpaceSeparatingPlane_[Variable]

class pydrake.geometry.optimization.CSpaceSeparatingPlane_[Variable]

Wraps the information that a pair of collision geometries are separated by a plane. One collision geometry is on the “positive” side of the separating plane, namely {x| aᵀx + b ≥ δ} (with δ ≥ 0}, and the other collision geometry is on the “negative” side of the separating plane, namely {x|aᵀx+b ≤ −δ}.

Template parameter T:

The type of decision_variables. T= symbolic::Variable or double.

__init__(*args, **kwargs)
property a
property b
property decision_variables
property expressed_body
property negative_side_geometry
property plane_degree
property positive_side_geometry
class pydrake.geometry.optimization.FindSeparationCertificateOptions
__init__(self: pydrake.geometry.optimization.FindSeparationCertificateOptions) None
property parallelism
property solver_id
property solver_options
property terminate_at_failure
property verbose
class pydrake.geometry.optimization.GcsGraphvizOptions
__init__(self: pydrake.geometry.optimization.GcsGraphvizOptions, **kwargs) None
property precision

Sets the floating point precision (how many digits are generated) of the annotations.

property scientific

Sets the floating point formatting to scientific (if true) or fixed (if false).

property show_costs

Determines whether the cost value results are shown. This will show both edge and vertex costs.

property show_flows

Determines whether the flow value results are shown. The flow values are shown both with a numeric value and through the transparency value on the edge, where a flow of 0.0 will correspond to an (almost) invisible edge, and a flow of 1.0 will display as a fully black edge.

property show_slacks

Determines whether the values of the intermediate (slack) variables are also displayed in the graph.

property show_vars

Determines whether the solution values for decision variables in each set are shown.

class pydrake.geometry.optimization.GraphOfConvexSets

GraphOfConvexSets (GCS) implements the design pattern and optimization problems first introduced in the paper “Shortest Paths in Graphs of Convex Sets”.

“Shortest Paths in Graphs of Convex Sets” by Tobia Marcucci, Jack Umenberger, Pablo A. Parrilo, Russ Tedrake. https://arxiv.org/abs/2101.11565

Warning

This feature is considered to be experimental and may change or be removed at any time, without any deprecation notice ahead of time.

Each vertex in the graph is associated with a convex set over continuous variables, edges in the graph contain convex costs and constraints on these continuous variables. We can then formulate optimization problems over this graph, such as the shortest path problem where each visit to a vertex also corresponds to selecting an element from the convex set subject to the costs and constraints. Behind the scenes, we construct efficient mixed-integer convex transcriptions of the graph problem using MathematicalProgram. However, we provide the option to solve an often tight convex relaxation of the problem with GraphOfConvexSetsOptions::convex_relaxation and employ a cheap rounding stage which solves the convex restriction along potential paths to find a feasible solution to the original problem.

Design note: This class avoids providing any direct access to the MathematicalProgram that it constructs nor to the decision variables / constraints. The users should be able to write constraints against “placeholder” decision variables on the vertices and edges, but these get translated in non-trivial ways to the underlying program.

Advanced Usage: Guiding Non-convex Optimization with the GraphOfConvexSets

Solving a GCS problem using convex relaxation involves two components: - Convex Relaxation: The relaxation of the binary variables (edge activations) and perspective operations on the convex cost/constraints leads to a convex problem that considers the graph as a whole. - Rounding: After solving the relaxation, a randomized rounding scheme is applied to obtain a feasible solution for the original problem. We interpret the relaxed flow variables as edge probabilities to guide the maximum likelyhood depth first search from the source to target vertices. Each rounding is calling SolveConvexRestriction.

To handle non-convex constraints, one can provide convex surrogates to the relaxation and the true non-convex constraints to the rounding problem. These surrogates approximate the non-convex constraints, making the relaxation solvable as a convex optimization to guide the non-convex rounding. This can be controlled by the Transcription enum in the AddConstraint method. We encourage users to provide a strong convex surrogate, when possible, to better approximate the original non-convex problem.

Users can also specify a GCS implicitly, which can be important for very large or infinite graphs, by deriving from ImplicitGraphOfConvexSets.

__init__(self: pydrake.geometry.optimization.GraphOfConvexSets) None

Constructs an empty graph.

AddEdge(self: pydrake.geometry.optimization.GraphOfConvexSets, u: pydrake.geometry.optimization.GraphOfConvexSets.Vertex, v: pydrake.geometry.optimization.GraphOfConvexSets.Vertex, name: str = '') pydrake.geometry.optimization.GraphOfConvexSets.Edge

Adds an edge to the graph from Vertex u to Vertex v. The vertex references must refer to valid vertices in this graph. If name is empty then a default name will be provided.

AddVertex(self: pydrake.geometry.optimization.GraphOfConvexSets, set: pydrake.geometry.optimization.ConvexSet, name: str = '') pydrake.geometry.optimization.GraphOfConvexSets.Vertex

Adds a vertex to the graph. A copy of set is cloned and stored inside the graph. If name is empty then a default name will be provided.

ClearAllPhiConstraints(self: pydrake.geometry.optimization.GraphOfConvexSets) None

Removes all constraints added to any edge with AddPhiConstraint.

class Edge

An edge in the graph connects between vertex u and vertex v. The edge also holds a list of cost and constraints associated with the continuous variables.

__init__(*args, **kwargs)
AddConstraint(*args, **kwargs)

Overloaded function.

  1. AddConstraint(self: pydrake.geometry.optimization.GraphOfConvexSets.Edge, f: pydrake.symbolic.Formula, use_in_transcription: set[pydrake.geometry.optimization.GraphOfConvexSets.Transcription] = {<Transcription.kMIP: 0>, <Transcription.kRelaxation: 1>, <Transcription.kRestriction: 2>}) -> pydrake.solvers.Binding[Constraint]

Adds a constraint to this edge.

Parameter f:

must contain only elements of xu() and xv() as variables.

Parameter use_in_transcription:

specifies the components of the problem to which the constraint should be added.

Raises
  • RuntimeError if f.GetFreeVariables() is not a subset of xu()

  • xv()

  • RuntimeError if xu() ∪ xv() is empty, i.e., when both vertices

  • have an ambient dimension of zero.

  • RuntimeError if no transcription is specified.

  1. AddConstraint(self: pydrake.geometry.optimization.GraphOfConvexSets.Edge, binding: pydrake.solvers.Binding[Constraint], use_in_transcription: set[pydrake.geometry.optimization.GraphOfConvexSets.Transcription] = {<Transcription.kMIP: 0>, <Transcription.kRelaxation: 1>, <Transcription.kRestriction: 2>}) -> pydrake.solvers.Binding[Constraint]

Adds a constraint to this edge.

Parameter bindings:

must contain only elements of xu() and xv() as variables.

Parameter use_in_transcription:

specifies the components of the problem to which the constraint should be added.

Raises
  • RuntimeError if binding.variables() is not a subset of xu()

  • xv()

  • RuntimeError if xu() ∪ xv() is empty, i.e., when both vertices

  • have an ambient dimension of zero.

  • RuntimeError if no transcription is specified.

AddCost(*args, **kwargs)

Overloaded function.

  1. AddCost(self: pydrake.geometry.optimization.GraphOfConvexSets.Edge, e: pydrake.symbolic.Expression, use_in_transcription: set[pydrake.geometry.optimization.GraphOfConvexSets.Transcription] = {<Transcription.kMIP: 0>, <Transcription.kRelaxation: 1>, <Transcription.kRestriction: 2>}) -> pydrake.solvers.Binding[Cost]

Adds a cost to this edge, described by a symbolic::Expression e containing only elements of xu() and xv() as variables. For technical reasons relating to being able to “turn-off” the cost on inactive edges, all costs are eventually implemented with a slack variable and a constraint:

Click to expand C++ code...
min g(xu, xv) ⇒ min ℓ, s.t. ℓ ≥ g(xu,xv)

You must use GetSolutionCost() to retrieve the cost of the solution, rather than evaluating the cost directly, in order to get consistent behavior when solving with the different GCS transcriptions.

Parameter use_in_transcription:

specifies the components of the problem to which the constraint should be added.

Note

Linear costs lead to negative costs if decision variables are not properly constrained. Users may want to check that the solution does not contain negative costs.

Returns

the added cost, g(xu, xv).

Raises
  • RuntimeError if e.GetVariables() is not a subset of xu() ∪ xv()

  • RuntimeError if no transcription is specified.

  1. AddCost(self: pydrake.geometry.optimization.GraphOfConvexSets.Edge, binding: pydrake.solvers.Binding[Cost], use_in_transcription: set[pydrake.geometry.optimization.GraphOfConvexSets.Transcription] = {<Transcription.kMIP: 0>, <Transcription.kRelaxation: 1>, <Transcription.kRestriction: 2>}) -> pydrake.solvers.Binding[Cost]

Adds a cost to this edge. binding must contain only elements of xu() and xv() as variables. For technical reasons relating to being able to “turn-off” the cost on inactive edges, all costs are eventually implemented with a slack variable and a constraint:

Click to expand C++ code...
min g(xu, xv) ⇒ min ℓ, s.t. ℓ ≥ g(xu,xv)

You must use GetSolutionCost() to retrieve the cost of the solution, rather than evaluating the cost directly, in order to get consistent behavior when solving with the different GCS transcriptions.

Parameter use_in_transcription:

specifies the components of the problem to which the constraint should be added.

Note

Linear costs lead to negative costs if decision variables are not properly constrained. Users may want to check that the solution does not contain negative costs.

Returns

the added cost, g(xu, xv).

Raises
  • RuntimeError if binding.variables() is not a subset of xu()

  • xv()

  • RuntimeError if no transcription is specified.

AddPhiConstraint(self: pydrake.geometry.optimization.GraphOfConvexSets.Edge, phi_value: bool) None

Adds a constraint on the binary variable associated with this edge.

Note

We intentionally do not return a binding to the constraint created by this call, as that would allow the caller to make nonsensical modifications to its bounds (i.e. requiring phi == 0.5).

ClearPhiConstraints(self: pydrake.geometry.optimization.GraphOfConvexSets.Edge) None

Removes any constraints added with AddPhiConstraint.

GetConstraints(self: pydrake.geometry.optimization.GraphOfConvexSets.Edge, used_in_transcription: set[pydrake.geometry.optimization.GraphOfConvexSets.Transcription] = {<Transcription.kMIP: 0>, <Transcription.kRelaxation: 1>, <Transcription.kRestriction: 2>}) list[pydrake.solvers.Binding[Constraint]]

Returns constraints on this edge.

Parameter used_in_transcription:

specifies the components of the problem from which the constraint should be retrieved.

Raises

RuntimeError if no transcription is specified.

GetCosts(self: pydrake.geometry.optimization.GraphOfConvexSets.Edge, used_in_transcription: set[pydrake.geometry.optimization.GraphOfConvexSets.Transcription] = {<Transcription.kMIP: 0>, <Transcription.kRelaxation: 1>, <Transcription.kRestriction: 2>}) list[pydrake.solvers.Binding[Cost]]

Returns costs on this edge.

Parameter used_in_transcription:

specifies the components of the problem from which the constraint should be retrieved.

Raises

RuntimeError if no transcription is specified.

GetSolutionCost(*args, **kwargs)

Overloaded function.

  1. GetSolutionCost(self: pydrake.geometry.optimization.GraphOfConvexSets.Edge, result: pydrake.solvers.MathematicalProgramResult) -> Optional[float]

Returns the sum of the costs associated with this edge in result, or std::nullopt if no solution for this edge is available.

  1. GetSolutionCost(self: pydrake.geometry.optimization.GraphOfConvexSets.Edge, result: pydrake.solvers.MathematicalProgramResult, cost: pydrake.solvers.Binding[Cost]) -> Optional[float]

Returns the cost associated with the cost binding on this edge in result, or std::nullopt if no solution for this edge is available.

Raises

RuntimeError if cost is not associated with this edge.

GetSolutionPhiXu(self: pydrake.geometry.optimization.GraphOfConvexSets.Edge, result: pydrake.solvers.MathematicalProgramResult) Optional[numpy.ndarray[numpy.float64[m, 1]]]

Returns the vector value of the slack variables associated with ϕxᵤ in result, or std::nullopt if no solution for this edge is available. This can obtain a different value than the Vertex::GetSolution(), e.g. from edge->xu().GetSolution(result). First, a deactivated edge (defined by Phi ~= 0) will return the zero vector here, while Vertex::GetSolution() will return std::nullopt (rather than divide by zero to recover Xu). Second, in the case of a loose convex relaxation, the vertex version will return the averaged* value of the edge slacks for all non-zero-flow edges.

GetSolutionPhiXv(self: pydrake.geometry.optimization.GraphOfConvexSets.Edge, result: pydrake.solvers.MathematicalProgramResult) Optional[numpy.ndarray[numpy.float64[m, 1]]]

Returns the vector value of the slack variables associated with ϕxᵥ in result, or std::nullopt if no solution for this edge is available. See GetSolutionPhiXu() for more details.

id(self: pydrake.geometry.optimization.GraphOfConvexSets.Edge) pydrake.geometry.optimization.GraphOfConvexSets.EdgeId

Returns the unique identifier associated with this Edge.

name(self: pydrake.geometry.optimization.GraphOfConvexSets.Edge) str

Returns the string name associated with this edge.

phi(self: pydrake.geometry.optimization.GraphOfConvexSets.Edge) pydrake.symbolic.Variable

Returns the binary variable associated with this edge. It can be used to determine whether this edge was active in the solution to an optimization problem, by calling GetSolution(phi()) on a returned MathematicalProgramResult.

u(self: pydrake.geometry.optimization.GraphOfConvexSets.Edge) pydrake.geometry.optimization.GraphOfConvexSets.Vertex

Returns a mutable reference to the “left” Vertex that this edge connects to.

v(self: pydrake.geometry.optimization.GraphOfConvexSets.Edge) pydrake.geometry.optimization.GraphOfConvexSets.Vertex

Returns a mutable reference to the “right” Vertex that this edge connects to.

xu(self: pydrake.geometry.optimization.GraphOfConvexSets.Edge) numpy.ndarray[object[m, 1]]

Returns the continuous decision variables associated with vertex u. This can be used for constructing symbolic::Expression costs and constraints.

See also GetSolutionPhiXu(); using result.GetSolution(xu()) may not be what you want.

xv(self: pydrake.geometry.optimization.GraphOfConvexSets.Edge) numpy.ndarray[object[m, 1]]

Returns the continuous decision variables associated with vertex v. This can be used for constructing symbolic::Expression costs and constraints.

See also GetSolutionPhiXv(); using result.GetSolution(xv()) may not be what you want.

class EdgeId
__init__(*args, **kwargs)
static get_new_id() pydrake.geometry.optimization.GraphOfConvexSets.EdgeId

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.optimization.GraphOfConvexSets.EdgeId) 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.optimization.GraphOfConvexSets.EdgeId) bool

Reports if the id is valid.

Edges(self: pydrake.geometry.optimization.GraphOfConvexSets) list[pydrake.geometry.optimization.GraphOfConvexSets.Edge]

Returns mutable pointers to the edges stored in the graph.

GetGraphvizString(*args, **kwargs)

Overloaded function.

  1. GetGraphvizString(self: pydrake.geometry.optimization.GraphOfConvexSets, result: pydrake.solvers.MathematicalProgramResult = None, options: pydrake.geometry.optimization.GcsGraphvizOptions = GcsGraphvizOptions(show_slacks=True, show_vars=True, show_flows=True, show_costs=True, scientific=False, precision=3), active_path: list[pydrake.geometry.optimization.GraphOfConvexSets.Edge] = []) -> str

Returns a Graphviz string describing the graph vertices and edges. If result is supplied, then the graph will be annotated with the solution values, according to options.

Parameter result:

the optional result from a solver.

Parameter options:

the struct containing various options for visualization.

Parameter active_path:

optionally highlights a given path in the graph. The path is displayed as dashed edges in red, displayed in addition to the original graph edges.

  1. GetGraphvizString(self: pydrake.geometry.optimization.GraphOfConvexSets, result: pydrake.solvers.MathematicalProgramResult = None, show_slacks: bool = True, show_vars: bool = True, show_flows: bool = True, show_costs: bool = True, scientific: bool = False, precision: int = 3, active_path: list[pydrake.geometry.optimization.GraphOfConvexSets.Edge] = []) -> str

Returns a Graphviz string describing the graph vertices and edges. If result is supplied, then the graph will be annotated with the solution values, according to options.

Parameter result:

the optional result from a solver.

Parameter options:

the struct containing various options for visualization.

Parameter active_path:

optionally highlights a given path in the graph. The path is displayed as dashed edges in red, displayed in addition to the original graph edges.

GetSolutionPath(self: pydrake.geometry.optimization.GraphOfConvexSets, source: pydrake.geometry.optimization.GraphOfConvexSets.Vertex, target: pydrake.geometry.optimization.GraphOfConvexSets.Vertex, result: pydrake.solvers.MathematicalProgramResult, tolerance: float = 0.001) list[pydrake.geometry.optimization.GraphOfConvexSets.Edge]

Extracts a path from source to target described by the result returned by SolveShortestPath(), via depth-first search following the largest values of the edge binary variables.

Parameter tolerance:

defines the threshold for checking the integrality conditions of the binary variables for each edge. tolerance = 0 would demand that the binary variables are exactly 1 for the edges on the path. tolerance = 1 would allow the binary variables to be any value in [0, 1]. The default value is 1e-3.

Raises
  • RuntimeError if !result.is_success() or no path from source to

  • target` can be found in the solution

IsValid(*args, **kwargs)

Overloaded function.

  1. IsValid(self: pydrake.geometry.optimization.GraphOfConvexSets, v: pydrake.geometry.optimization.GraphOfConvexSets.Vertex) -> bool

Returns true iff v is registered as a vertex with this.

  1. IsValid(self: pydrake.geometry.optimization.GraphOfConvexSets, e: pydrake.geometry.optimization.GraphOfConvexSets.Edge) -> bool

Returns true iff e is registered as an edge with this.

num_edges(self: pydrake.geometry.optimization.GraphOfConvexSets) int
num_vertices(self: pydrake.geometry.optimization.GraphOfConvexSets) int
RemoveEdge(self: pydrake.geometry.optimization.GraphOfConvexSets, edge: pydrake.geometry.optimization.GraphOfConvexSets.Edge) None

Removes edge edge from the graph.

Precondition:

The edge must be part of the graph.

RemoveVertex(self: pydrake.geometry.optimization.GraphOfConvexSets, vertex: pydrake.geometry.optimization.GraphOfConvexSets.Vertex) None

Removes vertex vertex from the graph as well as any edges from or to the vertex. Runtime is O(nₑ) where nₑ is the number of edges in the graph.

Precondition:

The vertex must be part of the graph.

SamplePaths(*args, **kwargs)

Overloaded function.

  1. SamplePaths(self: pydrake.geometry.optimization.GraphOfConvexSets, source: pydrake.geometry.optimization.GraphOfConvexSets.Vertex, target: pydrake.geometry.optimization.GraphOfConvexSets.Vertex, flows: dict[pydrake.geometry.optimization.GraphOfConvexSets.Edge, float], options: pydrake.geometry.optimization.GraphOfConvexSetsOptions) -> list[list[pydrake.geometry.optimization.GraphOfConvexSets.Edge]]

Samples a collection of unique paths from source to target, where the flow values (the relaxed binary variables associated with each Edge) flows are interpreted as the probabilities of transitioning an edge. The returned paths are guaranteed to be unique, and the number of returned paths can be 0 if no paths are found. This function implements the first part of the rounding scheme put forth in Section 4.2 of “Motion Planning around Obstacles with Convex Optimization”: https://arxiv.org/abs/2205.04422

Parameter source:

specifies the source vertex.

Parameter target:

specifies the target vertex.

Parameter flows:

specifies the edge flows, which are interprested as the probability of transition an edge. Edge flows that are not specified are taken to be zero.

Parameter options:

include all settings for sampling the paths. Specifically, the behavior of this function is determined through options.rounding_seed, options.max_rounded_paths, options.max_rounding_trials, and options.flow_tolerance, as described in GraphOfConvexSetsOptions.

Returns

A vector of paths, where each path is a vector of `Edge`s.

Raises

RuntimeError if options.max_rounded_path < 1.

  1. SamplePaths(self: pydrake.geometry.optimization.GraphOfConvexSets, source: pydrake.geometry.optimization.GraphOfConvexSets.Vertex, target: pydrake.geometry.optimization.GraphOfConvexSets.Vertex, result: pydrake.solvers.MathematicalProgramResult, options: pydrake.geometry.optimization.GraphOfConvexSetsOptions) -> list[list[pydrake.geometry.optimization.GraphOfConvexSets.Edge]]

Samples a collection of unique paths from source to target, where the flow values (the relaxed binary variables associated with each Edge) in result are interpreted as the probabilities of transitioning an edge. The returned paths are guaranteed to be unique, and the number of returned paths can be 0 if no paths are found. This function implements the first part of the rounding scheme put forth in Section 4.2 of “Motion Planning around Obstacles with Convex Optimization”: https://arxiv.org/abs/2205.04422

Parameter source:

specifies the source vertex.

Parameter target:

specifies the target vertex.

Parameter options:

include all settings for sampling the paths. Specifically, the behavior of this function is determined through options.rounding_seed, options.max_rounded_paths, options.max_rounding_trials, and options.flow_tolerance, as described in GraphOfConvexSetsOptions.

Returns

A vector of paths, where each path is a vector of `Edge`s.

Raises

RuntimeError if options.max_rounded_path < 1.

SolveConvexRestriction(self: pydrake.geometry.optimization.GraphOfConvexSets, active_edges: list[pydrake.geometry.optimization.GraphOfConvexSets.Edge], options: pydrake.geometry.optimization.GraphOfConvexSetsOptions = GraphOfConvexSetsOptions(convex_relaxation=None, preprocessing=None, max_rounded_paths=None, max_rounding_trials=100, flow_tolerance=1e-05, rounding_seed=0, solver=None, restriction_solver=None, preprocessing_solver=None, solver_options=<SolverOptions>, restriction_solver_options=None, preprocessing_solver_options=None, ), initial_guess: pydrake.solvers.MathematicalProgramResult = None) pydrake.solvers.MathematicalProgramResult

The non-convexity in a GCS problem comes from the binary variables (phi) associated with the edges being active or inactive in the solution. If those binary variables are fixed, then the problem is convex – this is a so-called “convex restriction” of the original problem.

The convex restriction can often be solved much more efficiently than solving the full GCS problem with additional constraints to fix the binaries; it can be written using less decision variables, and needs only to include the vertices associated with at least one of the active edges. Decision variables for all other convex sets will be set to NaN.

Note that one can specify additional non-convex constraints, which may be not supported by all solvers. In this case, the provided solver will throw an exception.

If an initial_guess is provided, the solution inside this result will be used to set the initial guess for the convex restriction. Typically, this will be the result obtained by solving the convex relaxation.

Raises
  • RuntimeError if the initial_guess does not contain solutions

  • for the decision variables required in this convex restriction.

SolveShortestPath(self: pydrake.geometry.optimization.GraphOfConvexSets, source: pydrake.geometry.optimization.GraphOfConvexSets.Vertex, target: pydrake.geometry.optimization.GraphOfConvexSets.Vertex, options: pydrake.geometry.optimization.GraphOfConvexSetsOptions = GraphOfConvexSetsOptions(convex_relaxation=None, preprocessing=None, max_rounded_paths=None, max_rounding_trials=100, flow_tolerance=1e-05, rounding_seed=0, solver=None, restriction_solver=None, preprocessing_solver=None, solver_options=<SolverOptions>, restriction_solver_options=None, preprocessing_solver_options=None, )) pydrake.solvers.MathematicalProgramResult

Formulates and solves the mixed-integer convex formulation of the shortest path problem on the graph, as discussed in detail in

“Shortest Paths in Graphs of Convex Sets” by Tobia Marcucci, Jack Umenberger, Pablo A. Parrilo, Russ Tedrake. https://arxiv.org/abs/2101.11565

Parameter source:

specifies the source set. The solver will choose any point in that set; to start at a particular continuous state consider adding a Point set to the graph and using that as the source.

Parameter target:

specifies the target set. The solver will choose any point in that set.

Parameter options:

include all settings for solving the shortest path problem. See GraphOfConvexSetsOptions for further details. The following default options will be used if they are not provided in options: - options.convex_relaxation = false, - options.max_rounded_paths = 0, - options.preprocessing = false.

Raises
  • RuntimeError if any of the costs or constraints in the graph are

  • incompatible with the shortest path formulation or otherwise

  • unsupported. All costs must be non-negative for all values of the

  • continuous variables.

class Transcription

Specify the transcription of the optimization problem to which a constraint or cost should be added, or from which they should be retrieved.

Members:

kMIP : The mixed integer formulation of the GCS problem.

kRelaxation : The relaxation of the GCS problem.

kRestriction : The restrction of the GCS problem where the path is fixed.

__init__(self: pydrake.geometry.optimization.GraphOfConvexSets.Transcription, value: int) None
kMIP = <Transcription.kMIP: 0>
kRelaxation = <Transcription.kRelaxation: 1>
kRestriction = <Transcription.kRestriction: 2>
property name
property value
class Vertex

Each vertex in the graph has a corresponding ConvexSet, and a std::string name.

__init__(*args, **kwargs)
AddConstraint(*args, **kwargs)

Overloaded function.

  1. AddConstraint(self: pydrake.geometry.optimization.GraphOfConvexSets.Vertex, f: pydrake.symbolic.Formula, use_in_transcription: set[pydrake.geometry.optimization.GraphOfConvexSets.Transcription] = {<Transcription.kMIP: 0>, <Transcription.kRelaxation: 1>, <Transcription.kRestriction: 2>}) -> pydrake.solvers.Binding[Constraint]

Adds a constraint to this vertex.

Parameter f:

must contain only elements of x() as variables.

Parameter use_in_transcription:

specifies the components of the problem to which the constraint should be added.

Raises
  • RuntimeError if f.GetFreeVariables() is not a subset of x()

  • RuntimeError if ambient_dimension() == 0.

  • RuntimeError if no transcription is specified.

  1. AddConstraint(self: pydrake.geometry.optimization.GraphOfConvexSets.Vertex, binding: pydrake.solvers.Binding[Constraint], use_in_transcription: set[pydrake.geometry.optimization.GraphOfConvexSets.Transcription] = {<Transcription.kMIP: 0>, <Transcription.kRelaxation: 1>, <Transcription.kRestriction: 2>}) -> pydrake.solvers.Binding[Constraint]

Adds a constraint to this vertex.

Parameter bindings:

must contain only elements of x() as variables.

Parameter use_in_transcription:

specifies the components of the problem to which the constraint should be added.

Raises
  • RuntimeError if binding.variables() is not a subset of x()

  • RuntimeError if ambient_dimension() == 0.

  • RuntimeError if no transcription is specified.

AddCost(*args, **kwargs)

Overloaded function.

  1. AddCost(self: pydrake.geometry.optimization.GraphOfConvexSets.Vertex, e: pydrake.symbolic.Expression, use_in_transcription: set[pydrake.geometry.optimization.GraphOfConvexSets.Transcription] = {<Transcription.kMIP: 0>, <Transcription.kRelaxation: 1>, <Transcription.kRestriction: 2>}) -> pydrake.solvers.Binding[Cost]

Adds a cost to this vertex, described by a symbolic::Expression e containing only elements of x() as variables. For technical reasons relating to being able to “turn-off” the cost on inactive vertices, all costs are eventually implemented with a slack variable and a constraint:

Click to expand C++ code...
min g(x) ⇒ min ℓ, s.t. ℓ ≥ g(x).

You must use GetSolutionCost() to retrieve the cost of the solution, rather than evaluating the cost directly, in order to get consistent behavior when solving with the different GCS transcriptions.

Parameter use_in_transcription:

specifies the components of the problem to which the constraint should be added.

Note

Linear costs lead to negative costs if decision variables are not properly constrained. Users may want to check that the solution does not contain negative costs.

Returns

the added cost, g(x).

Raises
  • RuntimeError if e.GetVariables() is not a subset of x()

  • RuntimeError if no transcription is specified.

  1. AddCost(self: pydrake.geometry.optimization.GraphOfConvexSets.Vertex, binding: pydrake.solvers.Binding[Cost], use_in_transcription: set[pydrake.geometry.optimization.GraphOfConvexSets.Transcription] = {<Transcription.kMIP: 0>, <Transcription.kRelaxation: 1>, <Transcription.kRestriction: 2>}) -> pydrake.solvers.Binding[Cost]

Adds a cost to this vertex. binding must contain only elements of x() as variables. For technical reasons relating to being able to “turn-off” the cost on inactive vertices, all costs are eventually implemented with a slack variable and a constraint:

Click to expand C++ code...
min g(x) ⇒ min ℓ, s.t. ℓ ≥ g(x).

You must use GetSolutionCost() to retrieve the cost of the solution, rather than evaluating the cost directly, in order to get consistent behavior when solving with the different GCS transcriptions.

Parameter use_in_transcription:

specifies the components of the problem to which the constraint should be added.

Note

Linear costs lead to negative costs if decision variables are not properly constrained. Users may want to check that the solution does not contain negative costs.

Returns

the added cost, g(x).

Raises
  • RuntimeError if binding.variables() is not a subset of x()

  • RuntimeError if no transcription is specified.

ambient_dimension(self: pydrake.geometry.optimization.GraphOfConvexSets.Vertex) int

Returns the ambient dimension of the ConvexSet.

GetConstraints(self: pydrake.geometry.optimization.GraphOfConvexSets.Vertex, used_in_transcription: set[pydrake.geometry.optimization.GraphOfConvexSets.Transcription] = {<Transcription.kMIP: 0>, <Transcription.kRelaxation: 1>, <Transcription.kRestriction: 2>}) list[pydrake.solvers.Binding[Constraint]]

Returns constraints on this vertex.

Parameter used_in_transcription:

specifies the components of the problem from which the constraint should be retrieved.

Raises

RuntimeError if no transcription is specified.

GetCosts(self: pydrake.geometry.optimization.GraphOfConvexSets.Vertex, used_in_transcription: set[pydrake.geometry.optimization.GraphOfConvexSets.Transcription] = {<Transcription.kMIP: 0>, <Transcription.kRelaxation: 1>, <Transcription.kRestriction: 2>}) list[pydrake.solvers.Binding[Cost]]

Returns costs on this vertex.

Parameter used_in_transcription:

specifies the components of the problem from which the constraint should be retrieved.

Raises

RuntimeError if no transcription is specified.

GetSolution(self: pydrake.geometry.optimization.GraphOfConvexSets.Vertex, result: pydrake.solvers.MathematicalProgramResult) Optional[numpy.ndarray[numpy.float64[m, 1]]]

Returns the solution of x() in result, or std::nullopt if no solution for this vertex is available. std::nullopt can happen if the vertex is deactivated (e.g. not in the shorest path) in the solution.

GetSolutionCost(*args, **kwargs)

Overloaded function.

  1. GetSolutionCost(self: pydrake.geometry.optimization.GraphOfConvexSets.Vertex, result: pydrake.solvers.MathematicalProgramResult) -> Optional[float]

Returns the sum of the costs associated with this vertex in result, or std::nullopt if no solution for this vertex is available.

  1. GetSolutionCost(self: pydrake.geometry.optimization.GraphOfConvexSets.Vertex, result: pydrake.solvers.MathematicalProgramResult, cost: pydrake.solvers.Binding[Cost]) -> Optional[float]

Returns the cost associated with the cost binding on this vertex in result, or std::nullopt if no solution for this vertex is available.

Raises

RuntimeError if cost is not associated with this vertex.

id(self: pydrake.geometry.optimization.GraphOfConvexSets.Vertex) pydrake.geometry.optimization.GraphOfConvexSets.VertexId

Returns the unique identifier associated with this Vertex.

incoming_edges(self: pydrake.geometry.optimization.GraphOfConvexSets.Vertex) list[drake::geometry::optimization::GraphOfConvexSets::Edge]
name(self: pydrake.geometry.optimization.GraphOfConvexSets.Vertex) str

Returns the name of the vertex.

outgoing_edges(self: pydrake.geometry.optimization.GraphOfConvexSets.Vertex) list[drake::geometry::optimization::GraphOfConvexSets::Edge]
set(self: pydrake.geometry.optimization.GraphOfConvexSets.Vertex) pydrake.geometry.optimization.ConvexSet

Returns a const reference to the underlying ConvexSet.

x(self: pydrake.geometry.optimization.GraphOfConvexSets.Vertex) numpy.ndarray[object[m, 1]]

Returns a decision variable corresponding to an element of the ConvexSet, which can be used for constructing symbolic::Expression costs and constraints.

class VertexId
__init__(*args, **kwargs)
static get_new_id() pydrake.geometry.optimization.GraphOfConvexSets.VertexId

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.optimization.GraphOfConvexSets.VertexId) 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.optimization.GraphOfConvexSets.VertexId) bool

Reports if the id is valid.

Vertices(self: pydrake.geometry.optimization.GraphOfConvexSets) list[pydrake.geometry.optimization.GraphOfConvexSets.Vertex]

Returns mutable pointers to the vertices stored in the graph.

class pydrake.geometry.optimization.GraphOfConvexSetsOptions
__init__(self: pydrake.geometry.optimization.GraphOfConvexSetsOptions) None
property convex_relaxation

Flag to solve the relaxed version of the problem. As discussed in the paper, we know that this relaxation cannot solve the original NP-hard problem for all instances, but there are also many instances for which the convex relaxation is tight. If convex_relaxation=nullopt, then each GCS method is free to choose an appropriate default.

property flow_tolerance

Tolerance for ignoring flow along a given edge during random rounding. If convex_relaxation is false or max_rounded_paths is less than or equal to zero, this option is ignored.

property max_rounded_paths

Maximum number of distinct paths to compare during random rounding; only the lowest cost path is returned. If convex_relaxation is false or this is less than or equal to zero, rounding is not performed. If max_rounded_paths=nullopt, then each GCS method is free to choose an appropriate default.

property max_rounding_trials

Maximum number of trials to find a novel path during random rounding. If convex_relaxation is false or max_rounded_paths is less than or equal to zero, this option is ignored.

property preprocessing

Performs a preprocessing step to remove edges that cannot lie on the path from source to target. In most cases, preprocessing causes a net reduction in computation by reducing the size of the optimization solved. Note that this preprocessing is not exact. There may be edges that cannot lie on the path from source to target that this does not detect. If preprocessing=nullopt, then each GCS method is free to choose an appropriate default.

property preprocessing_solver

Optimizer to be used in the preprocessing stage of GCS, which is performed when SolveShortestPath is called when the preprocessing setting has been set to true. If not set, the interface at .solver will be used, if provided, otherwise the best solver for the given problem is selected. Note that if the solver cannot handle the type of optimization problem generated, then calling the solvers::SolverInterface::Solve() method will throw.

property preprocessing_solver_options

Optional solver options to be used by preprocessing_solver in the preprocessing stage of GCS, which is used in SolveShortestPath. If preprocessing_solver is set but this parameter is not then solver_options is used. For instance, one might want to print solver logs for the main optimization, but not from the many smaller preprocessing optimizations.

property restriction_solver

Optimizer to be used in SolveConvexRestriction(), which is also called during the rounding stage of SolveShortestPath() given the relaxation. If not set, the interface at .solver will be used, if provided, otherwise the best solver for the given problem is selected. Note that if the solver cannot handle the type of optimization problem generated, then calling the solvers::SolverInterface::Solve() method will throw.

property restriction_solver_options

Optional solver options to be used in SolveConvexRestriction(), which is also used during the rounding stage of SolveShortestPath() given the relaxation. If not set, solver_options is used. For instance, one might want to set tighter (i.e., lower) tolerances for running the relaxed problem and looser (i.e., higher) tolerances for final solves during rounding.

property rounding_seed

Random seed to use for random rounding. If convex_relaxation is false or max_rounded_paths is less than or equal to zero, this option is ignored.

property solver

Optimizer to be used to solve the MIP, the relaxation of the shortest path optimization problem and the convex restriction if no restriction_solver is provided. If not set, the best solver for the given problem is selected. Note that if the solver cannot handle the type of optimization problem generated, the calling solvers::SolverInterface::Solve() method will throw.

property solver_options

Options passed to the solver when solving the generated problem.

class pydrake.geometry.optimization.HPolyhedron

Bases: pydrake.geometry.optimization.ConvexSet

Implements a polyhedral convex set using the half-space representation: {x| A x b}. Note: This set may be unbounded.

By convention, we treat a zero-dimensional HPolyhedron as nonempty.

__init__(*args, **kwargs)

Overloaded function.

  1. __init__(self: pydrake.geometry.optimization.HPolyhedron) -> None

Constructs a default (zero-dimensional, nonempty) polyhedron.

  1. __init__(self: pydrake.geometry.optimization.HPolyhedron, A: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], b: numpy.ndarray[numpy.float64[m, 1]]) -> None

Constructs the polyhedron.

Precondition:

A.rows() == b.size().

  1. __init__(self: pydrake.geometry.optimization.HPolyhedron, query_object: pydrake.geometry.QueryObject, geometry_id: pydrake.geometry.GeometryId, reference_frame: Optional[pydrake.geometry.FrameId] = None) -> None

Constructs a new HPolyhedron from a SceneGraph geometry and pose in the reference_frame frame, obtained via the QueryObject. If reference_frame frame is std::nullopt, then it will be expressed in the world frame.

Raises

RuntimeError the geometry is not a convex polytope.

  1. __init__(self: pydrake.geometry.optimization.HPolyhedron, vpoly: pydrake.geometry.optimization.VPolytope, tol: float = 1e-09) -> None

Constructs a new HPolyedron from a VPolytope object. This function will use qhull. If the VPolytope is empty, then the HPolyhedron will also be empty. If the HPolyhedron is not full-dimensional, we perform computations in a coordinate system of its affine hull. tol specifies the numerical tolerance used in the computation of the affine hull. (See the documentation of AffineSubspace.) A tighter tolerance can be used with commercial solvers (e.g. Gurobi and Mosek).

Raises

RuntimeError if vpoly is empty and zero dimensional.

  1. __init__(self: pydrake.geometry.optimization.HPolyhedron, prog: pydrake.solvers.MathematicalProgram) -> None

Constructs a new HPolyhedron describing the feasible set of a linear program prog. The i`th dimension in this representation corresponds to the `i`th decision variable of `prog. Note that if prog is infeasible, then the constructed HPolyhedron will be empty.

Raises
  • RuntimeError if prog has constraints which are not of type linear

  • inequality, linear equality, or bounding box.

A(self: pydrake.geometry.optimization.HPolyhedron) numpy.ndarray[numpy.float64[m, n]]

Returns the half-space representation matrix A.

b(self: pydrake.geometry.optimization.HPolyhedron) numpy.ndarray[numpy.float64[m, 1]]

Returns the half-space representation vector b.

CartesianPower(self: pydrake.geometry.optimization.HPolyhedron, n: int) pydrake.geometry.optimization.HPolyhedron

Returns the n-ary Cartesian power of this. The n-ary Cartesian power of a set H is the set H ⨉ H ⨉ … ⨉ H, where H is repeated n times.

CartesianProduct(self: pydrake.geometry.optimization.HPolyhedron, other: pydrake.geometry.optimization.HPolyhedron) pydrake.geometry.optimization.HPolyhedron

Returns the Cartesian product of this and other.

ChebyshevCenter(self: pydrake.geometry.optimization.HPolyhedron) numpy.ndarray[numpy.float64[m, 1]]

Solves a linear program to compute the center of the largest inscribed ball in the polyhedron. This is often the recommended way to find some interior point of the set, for example, as a step towards computing the convex hull or a vertex-representation of the set.

Note that the Chebyshev center is not necessarily unique, and may not conform to the point that one might consider the “visual center” of the set. For example, for a long thin rectangle, any point in the center line segment illustrated below would be a valid center point. The solver may return any point on that line segment.

Click to expand C++ code...
┌──────────────────────────────────┐
│                                  │
│   ────────────────────────────   │
│                                  │
└──────────────────────────────────┘

To find the visual center, consider using the more expensive MaximumVolumeInscribedEllipsoid() method, and then taking the center of the returned Hyperellipsoid.

Raises

RuntimeError if the solver fails to solve the problem.

ContainedIn(self: pydrake.geometry.optimization.HPolyhedron, other: pydrake.geometry.optimization.HPolyhedron, tol: float = 1e-09) bool

Returns true iff this HPolyhedron is entirely contained in the HPolyhedron other. This is done by checking whether every inequality in other is redundant when added to this.

Parameter tol:

We check if this polyhedron is contained in other.A().row(i).dot(x) <= other.b()(i) + tol. The larger tol value is, the more relaxation we add to the containment. If tol is negative, then we check if a shrinked other contains this polyheron.

FindRedundant(self: pydrake.geometry.optimization.HPolyhedron, tol: float = 1e-09) set[int]

Finds the redundant inequalities in this polyhedron. Returns a set ℑ, such that if we remove the rows of A * x <= b in ℑ, the remaining inequalities still define the same polyhedron, namely {x | A*x<=b} = {x | A.row(i)*x<=b(i), ∀i ∉ ℑ}. This function solves a series of linear programs. We say the jᵗʰ row A.row(j)*x <= b(j) is redundant, if {x | A.row(i) * x <= b(i), ∀i ∉ ℑ} implies that A.row(j) * x <= b(j) + tol. Note that we do NOT guarantee that we find all the redundant rows.

Intersection(self: pydrake.geometry.optimization.HPolyhedron, other: pydrake.geometry.optimization.HPolyhedron, check_for_redundancy: bool = False, tol: float = 1e-09) pydrake.geometry.optimization.HPolyhedron

Constructs the intersection of two HPolyhedron by adding the rows of inequalities from other. If check_for_redundancy is true then only adds the rows of other other.A().row(i).dot(x)<=other.b()(i) to this HPolyhedron if the inequality other.A().row(i).dot(x)<=other.b()(i)+tol is not implied by the inequalities from this HPolyhedron. A positive tol means it is more likely to deem a constraint being redundant and remove it. A negative tol means it is less likely to remove a constraint.

static MakeBox(lb: numpy.ndarray[numpy.float64[m, 1]], ub: numpy.ndarray[numpy.float64[m, 1]]) pydrake.geometry.optimization.HPolyhedron

Constructs a polyhedron as an axis-aligned box from the lower and upper corners.

static MakeL1Ball(dim: int) pydrake.geometry.optimization.HPolyhedron

Constructs the L1-norm unit ball in dim dimensions, {x | |x|₁ <= 1 }. This set is also known as the cross-polytope and is described by the 2ᵈⁱᵐ signed unit vectors.

static MakeUnitBox(dim: int) pydrake.geometry.optimization.HPolyhedron

Constructs the L∞-norm unit box in dim dimensions, {x | |x|∞ <= 1 }. This is an axis-aligned box, centered at the origin, with edge length 2.

MaximumVolumeInscribedAffineTransformation(self: pydrake.geometry.optimization.HPolyhedron, circumbody: pydrake.geometry.optimization.HPolyhedron) pydrake.geometry.optimization.HPolyhedron

Solves a semi-definite program to compute the maximum-volume affine transformation of this, subject to being a subset of circumbody, and subject to the transformation matrix being positive semi-definite. The latter condition is necessary for convexity of the program. We use the containment condition stated in Lemma 1 of “Linear Encodings for Polytope Containment Problems” by Sadra Sadraddini and Russ Tedrake, extended to apply to the affine transformation of this. We solve

Click to expand C++ code...
max_{T,t} log det (T)
s.t. T ≽ 0
t + TX ⊆ Y

where X is this, and Y is circumbody.

Returns

the transformed polyhedron, t + TX.

Parameter circumbody:

is an HPolyhedron that must contain the returned inbody.

Raises

RuntimeError if the solver fails to solve the problem.

MaximumVolumeInscribedEllipsoid(self: pydrake.geometry.optimization.HPolyhedron) pydrake.geometry.optimization.Hyperellipsoid

Solves a semi-definite program to compute the inscribed ellipsoid. This is also known as the inner Löwner-John ellipsoid. From Section 8.4.2 in Boyd and Vandenberghe, 2004, we solve

Click to expand C++ code...
max_{C,d} log det (C)
s.t. |aᵢC|₂ ≤ bᵢ - aᵢd, ∀i
C ≽ 0

where aᵢ and bᵢ denote the ith row. This defines the ellipsoid E = { Cx + d | |x|₂ ≤ 1}.

Precondition:

the HPolyhedron is bounded.

Raises

RuntimeError if the solver fails to solve the problem.

PontryaginDifference(self: pydrake.geometry.optimization.HPolyhedron, other: pydrake.geometry.optimization.HPolyhedron) pydrake.geometry.optimization.HPolyhedron

Returns the Pontryagin (Minkowski) Difference of this and other. This is the set A ⊖ B = { a|a+ B ⊆ A }. The result is an HPolyhedron with the same number of inequalities as A. Requires that this and other both be bounded and have the same ambient dimension. This method may throw a runtime error if this or other are ill-conditioned.

ReduceInequalities(self: pydrake.geometry.optimization.HPolyhedron, tol: float = 1e-09) pydrake.geometry.optimization.HPolyhedron

Reduces some (not necessarily all) redundant inequalities in the HPolyhedron. This is not guaranteed to give the minimal representation of the polyhedron but is a relatively fast way to reduce the number of inequalities.

Parameter tol:

For a constraint c’x<=d, if the halfspace c’x<=d + tol contains the hpolyhedron generated by the rest of the constraints, then we remove this inequality. A positive tol means it is more likely to remove a constraint, a negative tol means it is less likely to remote a constraint.

Scale(self: pydrake.geometry.optimization.HPolyhedron, scale: float, center: Optional[numpy.ndarray[numpy.float64[m, 1]]] = None) pydrake.geometry.optimization.HPolyhedron

Results a new HPolyhedron that is a scaled version of this, by scaling the distance from each face to the center by a factor of pow(scale, 1/ambient_dimension()), to have units of volume: - scale = 0 will result in a point, - 0 < scale < 1 shrinks the region, - scale = 1 returns a copy of the this, and - 1 < scale grows the region.

If center is not provided, then the value returned by ChebyshevCenter() will be used.

this does not need to be bounded, nor have volume. center does not need to be in the set.

Precondition:

scale >= 0.

Precondition:

center has size equal to the ambient dimension.

SimplifyByIncrementalFaceTranslation(self: pydrake.geometry.optimization.HPolyhedron, min_volume_ratio: float = 0.1, do_affine_transformation: bool = True, max_iterations: int = 10, points_to_contain: numpy.ndarray[numpy.float64[m, n]] = array([], shape=(0, 0), dtype=float64), intersecting_polytopes: list[pydrake.geometry.optimization.HPolyhedron] = [], keep_whole_intersection: bool = False, intersection_padding: float = 0.0001, random_seed: int = 0) pydrake.geometry.optimization.HPolyhedron

Returns an inner approximation of this, aiming to use fewer faces. Proceeds by incrementally translating faces inward and removing other faces that become redundant upon doing so.

Parameter min_volume_ratio:

is a lower bound for the ratio of the volume of the returned inbody and the volume of this.

Parameter do_affine_transformation:

specifies whether to call MaximumVolumeInscribedAffineTransformation(), to take an affine transformation of the inner approximation to maximize its volume. The affine transformation is reverted if the resulting inner approximation violates conditions related to points_to_contain or intersecting_polytopes.

Parameter max_iterations:

is the maximum number of times to loop through all faces.

Parameter points_to_contain:

is an optional matrix whose columns are points that must be contained in the returned inbody.

Parameter intersecting_polytopes:

is an optional list of HPolyhedrons that must intersect with the returned inbody.

Parameter keep_whole_intersection:

specifies whether the face translation step of the algorithm is prohibited from reducing the intersections with the HPolyhedrons in intersecting_polytopes. Regardless of the value of this parameter, the intersections may be reduced by the affine transformation step if do_affine_transformation is true.

Parameter intersection_padding:

is a distance by which each hyperplane is translated back outward after satisfing intersection constraints, subject to not surpassing the original hyperplane position. In the case where keep_whole_intersection is false, using a non-zero value for this parameter prevents intersections from being single points.

Parameter random_seed:

is a seed for a random number generator used to shuffle the ordering of hyperplanes in between iterations.

Precondition:

min_volume_ratio > 0.

Precondition:

max_iterations > 0.

Precondition:

intersection_padding >= 0.

Precondition:

All columns of points_to_contain are points contained within this.

Precondition:

All elements of intersecting_polytopes intersect with this.

UniformSample(*args, **kwargs)

Overloaded function.

  1. UniformSample(self: pydrake.geometry.optimization.HPolyhedron, generator: pydrake.common.RandomGenerator, previous_sample: numpy.ndarray[numpy.float64[m, 1]], mixing_steps: int = 10, subspace: Optional[numpy.ndarray[numpy.float64[m, n], flags.f_contiguous]] = None, tol: float = 1e-08) -> numpy.ndarray[numpy.float64[m, 1]]

Draw an (approximately) uniform sample from the set using the hit and run Markov-chain Monte-Carlo strategy described in https://link.springer.com/article/10.1007/s101070050099. To draw many samples from the uniform distribution, pass the output of one iteration in as the previous_sample to the next, with mixing_steps set to a relatively low number. When drawing a single sample, mixing_steps should be set relatively high in order to obtain an approximately uniformly random point. The distribution of samples will converge to the true uniform distribution at a geometric rate in the total number of hit-and-run steps which is mixing_steps * the number of times this function is called. If a subspace is provided, the random samples are constrained to lie in the affine subspace through previous_sample, spanned by the columns of subspace. To obtain uniform samples, subspace should have orthonormal, columns. This enables drawing uniform samples from an HPolyhedron which is not full-dimensional – one can pass the basis of the affine hull of the HPolyhedron, which can be computed with the AffineSubspace class. tol is a numerical tolerance for checking if any halfspaces in the given HPolyhedron are implied by the subspace definition (and therefore can be ignored by the hit-and-run sampler).

Precondition:

subspace.rows() == ambient_dimension().

Raises

RuntimeError if previous_sample is not in the set.

  1. UniformSample(self: pydrake.geometry.optimization.HPolyhedron, generator: pydrake.common.RandomGenerator, mixing_steps: int = 10, subspace: Optional[numpy.ndarray[numpy.float64[m, n], flags.f_contiguous]] = None, tol: float = 1e-08) -> numpy.ndarray[numpy.float64[m, 1]]

Variant of UniformSample that uses the ChebyshevCenter() as the previous_sample as a feasible point to start the Markov chain sampling.

class pydrake.geometry.optimization.Hyperellipsoid

Bases: pydrake.geometry.optimization.ConvexSet

Implements an ellipsoidal convex set represented by the quadratic form {x | (x-center)ᵀAᵀA(x-center) 1}. Note that A need not be square; we require only that the matrix AᵀA is positive semi-definite.

Compare this with an alternative (very useful) parameterization of the ellipsoid: {Bu + center | |u|₂ 1}, which is an affine scaling of the unit ball. This is related to the quadratic form by B = A⁻¹, when A is invertible, but the quadratic form can also represent unbounded sets. The affine scaling of the unit ball representation is available via the AffineBall class.

Note: the name Hyperellipsoid was taken here to avoid conflicting with geometry::Ellipsoid and to distinguish that this class supports N dimensions.

A hyperellipsoid can never be empty – it always contains its center. This includes the zero-dimensional case.

__init__(*args, **kwargs)

Overloaded function.

  1. __init__(self: pydrake.geometry.optimization.Hyperellipsoid) -> None

Constructs a default (zero-dimensional, nonempty) set.

  1. __init__(self: pydrake.geometry.optimization.Hyperellipsoid, A: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], center: numpy.ndarray[numpy.float64[m, 1]]) -> None

Constructs the ellipsoid.

Precondition:

A.cols() == center.size().

  1. __init__(self: pydrake.geometry.optimization.Hyperellipsoid, query_object: pydrake.geometry.QueryObject, geometry_id: pydrake.geometry.GeometryId, reference_frame: Optional[pydrake.geometry.FrameId] = None) -> None

Constructs a Hyperellipsoid from a SceneGraph geometry and pose in the reference_frame frame, obtained via the QueryObject. If reference_frame frame is std::nullopt, then it will be expressed in the world frame.

Raises
  • RuntimeError if geometry_id does not represent a shape that can be

  • described as an Hyperellipsoid.

  1. __init__(self: pydrake.geometry.optimization.Hyperellipsoid, ellipsoid: pydrake.geometry.optimization.AffineBall) -> None

Constructs a Hyperellipsoid from an AffineBall.

Precondition:

ellipsoid.B() is invertible.

A(self: pydrake.geometry.optimization.Hyperellipsoid) numpy.ndarray[numpy.float64[m, n]]

Returns the quadratic form matrix A.

center(self: pydrake.geometry.optimization.Hyperellipsoid) numpy.ndarray[numpy.float64[m, 1]]

Returns the center of the ellipsoid.

static MakeAxisAligned(radius: numpy.ndarray[numpy.float64[m, 1]], center: numpy.ndarray[numpy.float64[m, 1]]) pydrake.geometry.optimization.Hyperellipsoid

Constructs the an axis-aligned Hyperellipsoid with the implicit form (x₀-c₀)²/r₀² + (x₁-c₁)²/r₁² + … + (x_N - c_N)²/r_N² ≤ 1, where c is shorthand for center and r is shorthand for radius.

static MakeHypersphere(radius: float, center: numpy.ndarray[numpy.float64[m, 1]]) pydrake.geometry.optimization.Hyperellipsoid

Constructs a hypersphere with radius and center.

static MakeUnitBall(dim: int) pydrake.geometry.optimization.Hyperellipsoid

Constructs the L₂-norm unit ball in dim dimensions, {x | |x|₂ <= 1 }.

MinimumUniformScalingToTouch(self: pydrake.geometry.optimization.Hyperellipsoid, other: pydrake.geometry.optimization.ConvexSet) tuple[float, numpy.ndarray[numpy.float64[m, 1]]]

Computes the smallest uniform scaling of this ellipsoid for which it still intersects other. √ minₓ (x-center)ᵀAᵀA(x-center) s.t. x ∈ other. Note that if center ∈ other, then we expect scaling = 0 and x = center (up to precision).

Precondition:

other must have the same ambient_dimension as this.

Returns

the minimal scaling and the witness point, x, on other.

Raises
  • RuntimeError if other is empty.

  • RuntimeError if ambient_dimension() == 0

static MinimumVolumeCircumscribedEllipsoid(points: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], rank_tol: float = 1e-06) pydrake.geometry.optimization.Hyperellipsoid

Constructs the minimum-volume ellipsoid which contains all of the points. This is commonly referred to as the outer Löwner-John ellipsoid.

Parameter points:

is a d-by-n matrix, where d is the ambient dimension and each column represents one point.

Parameter rank_tol:

the singular values of the data matrix will be considered non-zero if they are strictly greater than rank_tol * max_singular_value. The default is 1e-6 to be compatible with common solver tolerances. This is used to detect if the data lies on a lower-dimensional affine space than the ambient dimension of the ellipsoid. If this is the case, then use AffineBall::MinimumVolumeCircumscribedEllipsoid instead.

Raises
  • RuntimeError if the MathematicalProgram fails to solve. If this

  • were to happen (due to numerical issues), then increasing

  • rank_tol` should provide a mitigation

  • RuntimeError if points includes NaNs or infinite values.

  • RuntimeError if the numerical data rank of points is less than d.

class pydrake.geometry.optimization.Hyperrectangle

Bases: pydrake.geometry.optimization.ConvexSet

Axis-aligned hyperrectangle in Rᵈ defined by its lower bounds and upper bounds as {x| lb ≤ x ≤ ub}

__init__(*args, **kwargs)

Overloaded function.

  1. __init__(self: pydrake.geometry.optimization.Hyperrectangle) -> None

Constructs a default (zero-dimensional, nonempty) hyperrectangle.

  1. __init__(self: pydrake.geometry.optimization.Hyperrectangle, lb: numpy.ndarray[numpy.float64[m, 1]], ub: numpy.ndarray[numpy.float64[m, 1]]) -> None

Constructs a hyperrectangle from its lower and upper bounds.

Precondition:

lb.size() == ub.size()

Precondition:

lb and ub are finite.

Precondition:

lb(i) <= ub(i) for all i

Center(self: pydrake.geometry.optimization.Hyperrectangle) numpy.ndarray[numpy.float64[m, 1]]

Get the center of the hyperrectangle.

lb(self: pydrake.geometry.optimization.Hyperrectangle) numpy.ndarray[numpy.float64[m, 1]]

Get the lower bounds of the hyperrectangle.

MakeHPolyhedron(self: pydrake.geometry.optimization.Hyperrectangle) pydrake.geometry.optimization.HPolyhedron

Helper to convert this hyperrectangle to an HPolyhedron.

static MaybeCalcAxisAlignedBoundingBox(set: pydrake.geometry.optimization.ConvexSet) Optional[pydrake.geometry.optimization.Hyperrectangle]

Returns the minimum axis-aligned bounding box of a convex set, for sets with finite volume. (std::nullopt otherwise).

MaybeGetIntersection(self: pydrake.geometry.optimization.Hyperrectangle, arg0: pydrake.geometry.optimization.Hyperrectangle) Optional[pydrake.geometry.optimization.Hyperrectangle]

Constructs the intersection of two Hyperrectangle by taking the pointwise maximum of the lower bounds and the pointwise minimums of the upper bounds. Returns std::nullopt if the intersection is empty.

Precondition:

this and other need to have the same ambient dimension.

ub(self: pydrake.geometry.optimization.Hyperrectangle) numpy.ndarray[numpy.float64[m, 1]]

Get the upper bounds of the hyperrectangle.

UniformSample(self: pydrake.geometry.optimization.Hyperrectangle, generator: pydrake.common.RandomGenerator) numpy.ndarray[numpy.float64[m, 1]]

Draws a uniform sample from the set.

class pydrake.geometry.optimization.ImplicitGraphOfConvexSets

A base class to define the interface to an implicit graph of convex sets.

Implementations of this class must implement DoSuccesors() and provide some method of accessing at least one vertex in the graph.

Warning

This feature is considered to be experimental and may change or be removed at any time, without any deprecation notice ahead of time.

__init__(self: pydrake.geometry.optimization.ImplicitGraphOfConvexSets) None

Constructs the (empty) implicit GCS.

ExpandRecursively(self: pydrake.geometry.optimization.ImplicitGraphOfConvexSets, start: pydrake.geometry.optimization.GraphOfConvexSets.Vertex, max_successor_calls: int = 1000) None

Makes repeated recursive calls to Successors() until no new vertices will be added to the graph, or max_successor_calls has been reached.

Note: v is mutable because exanding a vertex requires changes to the underlying vertex object.

Raises

RuntimeError if v is not already registered with the graph.

gcs(self: pydrake.geometry.optimization.ImplicitGraphOfConvexSets) pydrake.geometry.optimization.GraphOfConvexSets
mutable_gcs(self: pydrake.geometry.optimization.ImplicitGraphOfConvexSets) pydrake.geometry.optimization.GraphOfConvexSets
Successors(self: pydrake.geometry.optimization.ImplicitGraphOfConvexSets, v: pydrake.geometry.optimization.GraphOfConvexSets.Vertex) list[pydrake.geometry.optimization.GraphOfConvexSets.Edge]

Returns the outgoing edges from v, which defines the “successors” of v in the common notation of implicit graph search. The internal gcs() object is expanded as needed to include the edges (and the vertices they point to) that are returned.

Note: The input arguments are mutable because expanding a vertex requires changes to the underlying vertex object. Similarly, the output is mutable because callers will need to get mutable vertex pointers from the returned edges to expand them further.

Raises

RuntimeError if v is not already registered with the graph.

class pydrake.geometry.optimization.Intersection

Bases: pydrake.geometry.optimization.ConvexSet

A convex set that represents the intersection of multiple sets: S = X₁ ∩ X₂ ∩ … ∩ Xₙ = {x | x ∈ X₁, x ∈ X₂, …, x ∈ Xₙ}

Special behavior for IsEmpty: The intersection of zero sets (i.e. when we have sets_.size() == 0) is always nonempty. This includes the zero-dimensional case, which we treat as being {0}, the unique zero-dimensional vector space.

__init__(*args, **kwargs)

Overloaded function.

  1. __init__(self: pydrake.geometry.optimization.Intersection) -> None

Constructs a default (zero-dimensional, nonempty) set.

  1. __init__(self: pydrake.geometry.optimization.Intersection, sets: list[pydrake.geometry.optimization.ConvexSet]) -> None

Constructs the intersection from a vector of convex sets.

  1. __init__(self: pydrake.geometry.optimization.Intersection, setA: pydrake.geometry.optimization.ConvexSet, setB: pydrake.geometry.optimization.ConvexSet) -> None

Constructs the intersection from a pair of convex sets.

element(self: pydrake.geometry.optimization.Intersection, index: int) pydrake.geometry.optimization.ConvexSet

Returns a reference to the ConvexSet defining the index element in the intersection.

num_elements(self: pydrake.geometry.optimization.Intersection) int

The number of elements (or sets) used in the intersection.

pydrake.geometry.optimization.Iris(obstacles: list[pydrake.geometry.optimization.ConvexSet], sample: numpy.ndarray[numpy.float64[m, 1]], domain: pydrake.geometry.optimization.HPolyhedron, options: pydrake.geometry.optimization.IrisOptions = IrisOptions(require_sample_point_is_contained=False, iteration_limit=100, termination_threshold=0.02, relative_termination_threshold=0.001, configuration_space_margin=0.01, num_collision_infeasible_samples=5, configuration_obstacles [], prog_with_additional_constraints is not set, num_additional_constraint_infeasible_samples=5, random_seed=1234, mixing_steps=10)) pydrake.geometry.optimization.HPolyhedron

The IRIS (Iterative Region Inflation by Semidefinite programming) algorithm, as described in

R. L. H. Deits and R. Tedrake, “Computing large convex regions of obstacle-free space through semidefinite programming,” Workshop on the Algorithmic Fundamentals of Robotics, Istanbul, Aug. 2014. http://groups.csail.mit.edu/robotics-center/public_papers/Deits14.pdf

This algorithm attempts to locally maximize the volume of a convex polytope representing obstacle-free space given a sample point and list of convex obstacles. Rather than compute the volume of the polytope directly, the algorithm maximizes the volume of an inscribed ellipsoid. It alternates between finding separating hyperplanes between the ellipsoid and the obstacles and then finding a new maximum-volume inscribed ellipsoid.

Parameter obstacles:

is a vector of convex sets representing the occupied space.

Parameter sample:

provides a point in the space; the algorithm is initialized using a tiny sphere around this point. The algorithm is only guaranteed to succeed if this sample point is collision free (outside of all obstacles), but in practice the algorithm can often escape bad initialization (assuming the require_sample_point_is_contained option is false).

Parameter domain:

describes the total region of interest; computed IRIS regions will be inside this domain. It must be bounded, and is typically a simple bounding box (e.g. from HPolyhedron::MakeBox).

The obstacles, sample, and the domain must describe elements in the same ambient dimension (but that dimension can be any positive integer).

pydrake.geometry.optimization.IrisInConfigurationSpace(plant: drake::multibody::MultibodyPlant<double>, context: pydrake.systems.framework.Context, options: pydrake.geometry.optimization.IrisOptions = IrisOptions(require_sample_point_is_contained=False, iteration_limit=100, termination_threshold=0.02, relative_termination_threshold=0.001, configuration_space_margin=0.01, num_collision_infeasible_samples=5, configuration_obstacles [], prog_with_additional_constraints is not set, num_additional_constraint_infeasible_samples=5, random_seed=1234, mixing_steps=10)) pydrake.geometry.optimization.HPolyhedron

A variation of the Iris (Iterative Region Inflation by Semidefinite programming) algorithm which finds collision-free regions in the configuration space of plant.

See also

Iris for details on the original algorithm. This variant uses nonlinear optimization (instead of convex optimization) to find collisions in configuration space; each potential collision is probabilistically “certified” by restarting the nonlinear optimization from random initial seeds inside the candidate IRIS region until it fails to find a collision in options.num_collision_infeasible_samples consecutive attempts.

This method constructs a single Iris region in the configuration space of plant.

See also

planning::IrisInConfigurationSpaceFromCliqueCover for a method to automatically cover the configuration space with multiple Iris regions.

Parameter plant:

describes the kinematics of configuration space. It must be connected to a SceneGraph in a systems::Diagram.

Parameter context:

is a context of the plant. The context must have the positions of the plant set to the initialIRIS seed configuration.

Parameter options:

provides additional configuration options. In particular, increasing options.num_collision_infeasible_samples increases the chances that the IRIS regions are collision free but can also significantly increase the run-time of the algorithm. The same goes for options.num_additional_constraints_infeasible_samples.

Raises
  • RuntimeError if the sample configuration in context is

  • infeasible.

  • RuntimeError if termination_func is invalid on the domain. See

  • IrisOptions.termination_func for more details.

class pydrake.geometry.optimization.IrisOptions

Configuration options for the IRIS algorithm.

__init__(self: pydrake.geometry.optimization.IrisOptions, **kwargs) None
property bounding_region

Optionally allows the caller to restrict the space within which IRIS regions are allowed to grow. By default, IRIS regions are bounded by the domain argument in the case of Iris or the joint limits of the input plant in the case of IrisInConfigurationSpace. If this option is specified, IRIS regions will be confined to the intersection between the domain and bounding_region

property configuration_obstacles

For IRIS in configuration space, it can be beneficial to not only specify task-space obstacles (passed in through the plant) but also obstacles that are defined by convex sets in the configuration space. This option can be used to pass in such configuration space obstacles.

property configuration_space_margin

For IRIS in configuration space, we retreat by this margin from each C-space obstacle in order to avoid the possibility of requiring an infinite number of faces to approximate a curved boundary.

property iteration_limit

Maximum number of iterations.

property mixing_steps

The mixing_steps parameters is passed to HPolyhedron::UniformSample to control the total number of hit-and-run steps taken for each new random sample.

property num_additional_constraint_infeasible_samples

For each constraint in prog_with_additional_constraints, IRIS will search for a counter-example by formulating a (likely nonconvex) optimization problem. The initial guess for this optimization is taken by sampling uniformly inside the current IRIS region. This option controls the termination condition for that counter-example search, defining the number of consecutive failures to find a counter-example requested before moving on to the next constraint.

property num_collision_infeasible_samples

For each possible collision, IRIS will search for a counter-example by formulating a (likely nonconvex) optimization problem. The initial guess for this optimization is taken by sampling uniformly inside the current IRIS region. This option controls the termination condition for that counter-example search, defining the number of consecutive failures to find a counter-example requested before moving on to the next constraint.

property prog_with_additional_constraints

By default, IRIS in configuration space certifies regions for collision avoidance constraints and joint limits. This option can be used to pass additional constraints that should be satisfied by the IRIS region. We accept these in the form of a MathematicalProgram:

find q subject to g(q) ≤ 0.

The decision_variables() for the program are taken to define q. IRIS will silently ignore any costs in prog_with_additional_constraints, and will throw RuntimeError if it contains any unsupported constraints.

For example, one could create an InverseKinematics problem with rich kinematic constraints, and then pass InverseKinematics::prog() into this option.

property random_seed

The only randomization in IRIS is the random sampling done to find counter-examples for the additional constraints using in IrisInConfigurationSpace. Use this option to set the initial seed.

property relative_termination_threshold

IRIS will terminate if the change in the volume of the hyperellipsoid between iterations is less that this percent of the previous best volume. This termination condition can be disabled by setting to a negative value.

property require_sample_point_is_contained

The initial polytope is guaranteed to contain the point if that point is collision-free. However, the IRIS alternation objectives do not include (and can not easily include) a constraint that the original sample point is contained. Therefore, the IRIS paper recommends that if containment is a requirement, then the algorithm should simply terminate early if alternations would ever cause the set to not contain the point.

property solver_options

The SolverOptions used in the optimization program.

property starting_ellipse

The initial hyperellipsoid that IRIS will use for calculating hyperplanes in the first iteration. If no hyperellipsoid is provided, a small hypershpere centered at the given sample will be used.

property termination_threshold

IRIS will terminate if the change in the volume of the hyperellipsoid between iterations is less that this threshold. This termination condition can be disabled by setting to a negative value.

property verify_domain_boundedness

If the user knows the intersection of bounding_region and the domain (for IRIS) or plant joint limits (for IrisInConfigurationSpace) is bounded, setting this flag to False will skip the boundedness check that IRIS and IrisInConfigurationSpace perform (leading to a small speedup, as checking boundedness requires solving optimization problems). If the intersection turns out to be unbounded, this will lead to undefined behavior.

pydrake.geometry.optimization.LoadIrisRegionsYamlFile(filename: os.PathLike, child_name: Optional[str] = None) dict[str, pydrake.geometry.optimization.HPolyhedron]

Calls LoadYamlFile() to deserialize an IrisRegions object.

pydrake.geometry.optimization.MakeIrisObstacles(query_object: pydrake.geometry.QueryObject, reference_frame: Optional[pydrake.geometry.FrameId] = None) list[pydrake.geometry.optimization.ConvexSet]

Constructs ConvexSet representations of obstacles for IRIS in 3D using the geometry from a SceneGraph QueryObject. All geometry in the scene with a proximity role, both anchored and dynamic, are consider to be fixed obstacles frozen in the poses captured in the context used to create the QueryObject.

When multiple representations are available for a particular geometry (e.g. a Box can be represented as either an HPolyhedron or a VPolytope), then this method will prioritize the representation that we expect is most performant for the current implementation of the IRIS algorithm.

class pydrake.geometry.optimization.MinkowskiSum

Bases: pydrake.geometry.optimization.ConvexSet

A convex set that represents the Minkowski sum of multiple sets: S = X₁ ⨁ X₂ ⨁ … ⨁ Xₙ = {x₁ + x₂ + … + xₙ | x₁ ∈ X₁, x₂ ∈ X₂, …, xₙ ∈ Xₙ}

Special behavior for IsEmpty: The Minkowski sum of zero sets (i.e. when we have sets_.size() == 0) is treated as the singleton {0}, which is nonempty. This includes the zero-dimensional case.

__init__(*args, **kwargs)

Overloaded function.

  1. __init__(self: pydrake.geometry.optimization.MinkowskiSum) -> None

Constructs a default (zero-dimensional, nonempty) set.

  1. __init__(self: pydrake.geometry.optimization.MinkowskiSum, sets: list[pydrake.geometry.optimization.ConvexSet]) -> None

Constructs the sum from a vector of convex sets.

  1. __init__(self: pydrake.geometry.optimization.MinkowskiSum, setA: pydrake.geometry.optimization.ConvexSet, setB: pydrake.geometry.optimization.ConvexSet) -> None

Constructs the sum from a pair of convex sets.

  1. __init__(self: pydrake.geometry.optimization.MinkowskiSum, query_object: pydrake.geometry.QueryObject, geometry_id: pydrake.geometry.GeometryId, reference_frame: Optional[pydrake.geometry.FrameId] = None) -> None

Constructs a MinkowskiSum from a SceneGraph geometry and pose in the reference_frame frame, obtained via the QueryObject. If reference_frame frame is std::nullopt, then it will be expressed in the world frame.

Although in principle a MinkowskiSum can represent any ConvexSet as the sum of a single set, here we only support Capsule geometry, which will be represented as the (non-trivial) Minkowski sum of a sphere with a line segment. Most SceneGraph geometry types are supported by at least one of the ConvexSet class constructors.

Raises

RuntimeError if geometry_id does not correspond to a Capsule.

num_terms(self: pydrake.geometry.optimization.MinkowskiSum) int

The number of terms (or sets) used in the sum.

term(self: pydrake.geometry.optimization.MinkowskiSum, index: int) pydrake.geometry.optimization.ConvexSet

Returns a reference to the ConvexSet defining the index term in the sum.

pydrake.geometry.optimization.PartitionConvexSet(*args, **kwargs)

Overloaded function.

  1. PartitionConvexSet(convex_set: pydrake.geometry.optimization.ConvexSet, continuous_revolute_joints: list[int], epsilon: float = 1e-05) -> list[pydrake.geometry.optimization.ConvexSet]

Partitions a convex set into (smaller) convex sets whose union is the original set and that each respect the convexity radius as in CheckIfSatisfiesConvexityRadius. In practice, this is implemented as partitioning sets into pieces whose width is less than or equal to π-ϵ. Each entry in continuous_revolute_joints must be non-negative, less than num_positions, and unique.

Parameter epsilon:

is the ϵ value used for the convexity radius inequality. The partitioned sets are made by intersecting convex_set with axis-aligned bounding boxes that respect the convexity radius. These boxes are made to overlap by ϵ radians along each dimension, for numerical purposes.

Returns

the vector of convex sets that each respect convexity radius.

Raises
  • RuntimeError if ϵ <= 0 or ϵ >= π.

  • RuntimeError if the input convex set is unbounded along dimensions

  • corresponding to continuous revolute joints.

  • RuntimeError if continuous_revolute_joints has repeated entries,

  • or if any entry is outside the interval [0,

  • convex_set.ambient_dimension())

  1. PartitionConvexSet(convex_sets: list[pydrake.geometry.optimization.ConvexSet], continuous_revolute_joints: list[int], epsilon: float = 1e-05) -> list[pydrake.geometry.optimization.ConvexSet]

Function overload to take in a list of convex sets, and partition all so as to respect the convexity radius. Every set must be bounded and have the same ambient dimension. Each entry in continuous_revolute_joints must be non-negative, less than num_positions, and unique.

Raises
  • RuntimeError unless every ConvexSet in convex_sets has the same

  • ambient_dimension.

  • RuntimeError if ϵ <= 0 or ϵ >= π.

  • RuntimeError if any input convex set is unbounded along dimensions

  • corresponding to continuous revolute joints.

  • RuntimeError if continuous_revolute_joints has repeated entries,

  • or if any entry is outside the interval [0, ambient_dimension)

class pydrake.geometry.optimization.PlaneSide

Members:

kPositive

kNegative

__init__(self: pydrake.geometry.optimization.PlaneSide, value: int) None
kNegative = <PlaneSide.kNegative: 1>
kPositive = <PlaneSide.kPositive: 0>
property name
property value
class pydrake.geometry.optimization.Point

Bases: pydrake.geometry.optimization.ConvexSet

A convex set that contains exactly one element. Also known as a singleton or unit set.

This set is always nonempty, even in the zero-dimensional case.

__init__(*args, **kwargs)

Overloaded function.

  1. __init__(self: pydrake.geometry.optimization.Point) -> None

Constructs a default (zero-dimensional, nonempty) set.

  1. __init__(self: pydrake.geometry.optimization.Point, x: numpy.ndarray[numpy.float64[m, 1]]) -> None

Constructs a Point.

  1. __init__(self: pydrake.geometry.optimization.Point, query_object: pydrake.geometry.QueryObject, geometry_id: pydrake.geometry.GeometryId, reference_frame: Optional[pydrake.geometry.FrameId] = None, maximum_allowable_radius: float = 0.0) -> None

Constructs a Point from a SceneGraph geometry and pose in the reference_frame frame, obtained via the QueryObject. If reference_frame frame is std::nullopt, then it will be expressed in the world frame.

Raises
  • RuntimeError if geometry_id does not correspond to a Sphere or if

  • the Sphere has radius greater than maximum_allowable_radius.

set_x(self: pydrake.geometry.optimization.Point, x: numpy.ndarray[numpy.float64[m, 1]]) None

Changes the element x describing the set.

Precondition:

x must be of size ambient_dimension().

x(self: pydrake.geometry.optimization.Point) numpy.ndarray[numpy.float64[m, 1]]

Retrieves the point.

class pydrake.geometry.optimization.SampledVolume

The result of a volume calculation from CalcVolumeViaSampling().

__init__(*args, **kwargs)
property num_samples

The number of samples used to compute the volume estimate.

property rel_accuracy

An upper bound for the relative accuracy of the volume estimate. When not evaluated, this value is NaN.

property volume

The estimated volume of the set.

pydrake.geometry.optimization.SaveIrisRegionsYamlFile(filename: os.PathLike, regions: dict[str, pydrake.geometry.optimization.HPolyhedron], child_name: Optional[str] = None) None

Calls SaveYamlFile() to serialize an IrisRegions object.

class pydrake.geometry.optimization.SeparatingPlaneOrder

The separating plane aᵀx + b ≥ δ, aᵀx + b ≤ −δ has parameters a and b. These parameterize a polynomial function of s_for_plane with the specified order. s_for_plane is a sub set of the configuration-space variable s, please refer to the RationalForwardKinematics class or the paper above on the meaning of s.

Members:

kAffine : a and b are affine functions of s.

__init__(self: pydrake.geometry.optimization.SeparatingPlaneOrder, value: int) None
kAffine = <SeparatingPlaneOrder.kAffine: 1>
property name
property value
class pydrake.geometry.optimization.SeparationCertificateProgramBase
__init__(*args, **kwargs)
property plane_index
prog(self: pydrake.geometry.optimization.SeparationCertificateProgramBase) pydrake.solvers.MathematicalProgram
class pydrake.geometry.optimization.SeparationCertificateResultBase

We certify that a pair of geometries is collision free by finding the separating plane over a range of configuration. The Lagrangian multipliers used for certifying this condition will differ in derived classes. This struct contains the the separating plane {x | aᵀx+b=0 } and derived classes may store the Lagrangians certifying that the plane separates the two geometries in separating_planes()[plane_index] in the C-space region.

__init__(*args, **kwargs)
property a
property b
property plane_decision_var_vals
property result
class pydrake.geometry.optimization.Spectrahedron

Bases: pydrake.geometry.optimization.ConvexSet

Implements a spectrahedron (the feasible set of a semidefinite program). The ambient dimension of the set is N(N+1)/2; the number of variables required to describe the N-by-N semidefinite matrix.

By convention, a zero-dimensional spectrahedron is considered nonempty.

__init__(*args, **kwargs)

Overloaded function.

  1. __init__(self: pydrake.geometry.optimization.Spectrahedron) -> None

Default constructor (yields the zero-dimensional nonempty set).

  1. __init__(self: pydrake.geometry.optimization.Spectrahedron, prog: pydrake.solvers.MathematicalProgram) -> None

Constructs the spectrahedron from a MathematicalProgram.

Raises
  • RuntimeError if prog.required_capabilities() is not a subset

  • of supported_attributes()

class pydrake.geometry.optimization.VPolytope

Bases: pydrake.geometry.optimization.ConvexSet

A polytope described using the vertex representation. The set is defined as the convex hull of the vertices. The vertices are not guaranteed to be in any particular order, nor to be minimal (some vertices could be strictly in the interior of the set).

Note: Unlike the half-space representation, this definition means the set is always bounded (hence the name polytope, instead of polyhedron).

A VPolytope is empty if and only if it is composed of zero vertices, i.e., if vertices_.cols() == 0. This includes the zero-dimensional case. If vertices_.rows() == 0 but vertices_.cols() > 0, we treat this as having one or more copies of 0 in the zero-dimensional vector space {0}. If vertices_.rows() and vertices_.cols() are zero, we treat this as no points in {0}, which is empty.

__init__(*args, **kwargs)

Overloaded function.

  1. __init__(self: pydrake.geometry.optimization.VPolytope) -> None

Constructs a set with no vertices in the zero-dimensional space, which is empty (by convention).

  1. __init__(self: pydrake.geometry.optimization.VPolytope, vertices: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous]) -> None

Constructs the polytope from a d-by-n matrix, where d is the ambient dimension, and n is the number of vertices. The vertices do not have to be ordered, nor minimal (they can contain points inside their convex hull).

  1. __init__(self: pydrake.geometry.optimization.VPolytope, H: pydrake.geometry.optimization.HPolyhedron, tol: float = 1e-09) -> None

Constructs the polytope from a bounded polyhedron (using Qhull). If the HPolyhedron is not full-dimensional, we perform computations in a coordinate system of its affine hull. tol specifies the numerical tolerance used in the computation of the affine hull. See the documentation of AffineSubspace for more details. A loose tolerance is necessary for the built-in solvers, but a tighter tolerance can be used with commercial solvers (e.g. Gurobi and Mosek).

Raises
  • RuntimeError if H is unbounded or if Qhull terminates with an

  • error.

  1. __init__(self: pydrake.geometry.optimization.VPolytope, query_object: pydrake.geometry.QueryObject, geometry_id: pydrake.geometry.GeometryId, reference_frame: Optional[pydrake.geometry.FrameId] = None) -> None

Constructs the polytope from a SceneGraph geometry.

GetMinimalRepresentation(self: pydrake.geometry.optimization.VPolytope) pydrake.geometry.optimization.VPolytope

Creates a new VPolytope whose vertices are guaranteed to be minimal, i.e., if we remove any point from its vertices, then the convex hull of the remaining vertices is a strict subset of the polytope. In the 2D case the vertices of the new VPolytope are ordered counter-clockwise from the negative X axis. For all other cases an order is not guaranteed.

static MakeBox(lb: numpy.ndarray[numpy.float64[m, 1]], ub: numpy.ndarray[numpy.float64[m, 1]]) pydrake.geometry.optimization.VPolytope

Constructs a polyhedron as an axis-aligned box from the lower and upper corners.

static MakeUnitBox(dim: int) pydrake.geometry.optimization.VPolytope

Constructs the L∞-norm unit box in dim dimensions, {x | |x|∞ <= 1 }. This is an axis-aligned box, centered at the origin, with edge length 2.

vertices(self: pydrake.geometry.optimization.VPolytope) numpy.ndarray[numpy.float64[m, n]]

Returns the vertices in a d-by-n matrix, where d is the ambient dimension, and n is the number of vertices.

WriteObj(self: pydrake.geometry.optimization.VPolytope, filename: os.PathLike) None

Uses qhull to compute the Delaunay triangulation and then writes the vertices and faces to filename in the Wavefront Obj format. Note that the extension .obj is not automatically added to the filename.

Precondition:

ambient_dimension() == 3.