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.
__init__(self: pydrake.geometry.optimization.AffineBall) -> None
Constructs a default (zero-dimensional, nonempty) set.
__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().
__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.
- 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 forradius
.- 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
andcenter
.- 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.
- Parameter
- 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.
__init__(self: pydrake.geometry.optimization.AffineSubspace) -> None
Constructs a default (zero-dimensional, nonempty) affine subspace.
__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().
__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. Passstd::nullopt
fortol
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. Passstd::nullopt
fortol
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.
- ContainedIn(self: pydrake.geometry.optimization.AffineSubspace, other: pydrake.geometry.optimization.AffineSubspace, tol: float = 1e-15) bool
Returns
True
ifthis
AffineSubspace is contained inother
. This is computed by checking iftranslation()
is inother
and then checking if each basis vector is in the span of the basis ofother
. 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
ifthis
AffineSubspace is contained inother
. This is computed by checking iftranslation()
is inother
and then checking if each basis vector is in the span of the basis ofother
. 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.
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 inconvex_sets_B
, and offset_A_to_B is is the translation to applied to all the points in the index_A’th set inconvex_sets_A
to align them with the index_B’th set inconvex_sets_B
. This translation may only have non-zero entries along the dimensions corresponding tocontinuous_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 incontinuous_revolute_joints
.- Parameter
convex_sets_A
: is a vector of convex sets. Pairwise intersections will be computed between
convex_sets_A
andconvex_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.
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
asbboxes_A
does toconvex_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.
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.
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.
__init__(self: pydrake.geometry.optimization.CartesianProduct) -> None
Constructs a default (zero-dimensional, nonempty) set.
__init__(self: pydrake.geometry.optimization.CartesianProduct, sets: list[pydrake.geometry.optimization.ConvexSet]) -> None
Constructs the product from a vector of convex sets.
__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.
__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. –
__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. Ifreference_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
- 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.”
- 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.
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 inconvex_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 inconvex_sets_A
to align them with the index_B’th set inconvex_sets_B
. This translation may only have non-zero entries along the dimensions corresponding tocontinuous_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 incontinuous_revolute_joints
.- Parameter
convex_sets_A
: is a vector of convex sets. Pairwise intersections will be computed between
convex_sets_A
andconvex_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. –
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
asbboxes_A
does toconvex_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 –
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. –
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.
- Parameter
- 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.
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 –
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 usingA*x = b
, whereA
has linearly independent rows, then the ambient dimension is the dimension ofx
, but the affine dimension of the set isambient_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. –
- Parameter
- 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
andother
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.
- Parameter
- 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.
- Parameter
- 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.
- Parameter
- 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.
- Parameter
- 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 parameter
- 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
- Template parameter
- 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 Vertexv
. The vertex references must refer to valid vertices in this graph. Ifname
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. Ifname
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 vertexv
. The edge also holds a list of cost and constraints associated with the continuous variables.- __init__(*args, **kwargs)
- AddConstraint(*args, **kwargs)
Overloaded function.
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. –
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.
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. –
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. –
- Parameter
- 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. –
- Parameter
- GetSolutionCost(*args, **kwargs)
Overloaded function.
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.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 inresult
, 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. fromedge->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.
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 tooptions
.- 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.
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 tooptions
.- 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
totarget
described by theresult
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 –
- Parameter
- IsValid(*args, **kwargs)
Overloaded function.
IsValid(self: pydrake.geometry.optimization.GraphOfConvexSets, v: pydrake.geometry.optimization.GraphOfConvexSets.Vertex) -> bool
Returns true iff
v
is registered as a vertex withthis
.IsValid(self: pydrake.geometry.optimization.GraphOfConvexSets, e: pydrake.geometry.optimization.GraphOfConvexSets.Edge) -> bool
Returns true iff
e
is registered as an edge withthis
.
- 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.
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
totarget
, where the flow values (the relaxed binary variables associated with eachEdge
) 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
, andoptions.flow_tolerance
, as described inGraphOfConvexSetsOptions
.
- Returns
A vector of paths, where each path is a vector of `Edge`s.
- Raises
RuntimeError if options.max_rounded_path < 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
totarget
, where the flow values (the relaxed binary variables associated with eachEdge
) inresult
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
, andoptions.flow_tolerance
, as described inGraphOfConvexSetsOptions
.
- 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 inoptions
: - 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. –
- Parameter
- 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.
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. –
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.
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. –
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. –
- Parameter
- 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. –
- Parameter
- 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.
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.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 inresult
, 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.
__init__(self: pydrake.geometry.optimization.HPolyhedron) -> None
Constructs a default (zero-dimensional, nonempty) polyhedron.
__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().
__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. Ifreference_frame
frame is std::nullopt, then it will be expressed in the world frame.- Raises
RuntimeError the geometry is not a convex polytope. –
__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. –
__init__(self: pydrake.geometry.optimization.HPolyhedron, prog: pydrake.solvers.MathematicalProgram) -> None
Constructs a new HPolyhedron describing the feasible set of a linear program
prog
. Thei`th dimension in this representation corresponds to the `i`th decision variable of `prog
. Note that ifprog
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 ofthis
. 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
andother
.
- 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.
- Parameter
- 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
. Ifcheck_for_redundancy
is true then only adds the rows ofother
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 ofcircumbody
, 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 ofthis
. We solveClick to expand C++ code...
max_{T,t} log det (T) s.t. T ≽ 0 t + TX ⊆ Y
where X is
this
, and Y iscircumbody
.- 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
andother
. This is the set A ⊖ B = { a|a+ B ⊆ A }. The result is an HPolyhedron with the same number of inequalities as A. Requires thatthis
andother
both be bounded and have the same ambient dimension. This method may throw a runtime error ifthis
orother
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.
- Parameter
- 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 thecenter
by a factor ofpow(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 thethis
, 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
orintersecting_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 ifdo_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 withinthis
.- Precondition:
All elements of
intersecting_polytopes
intersect withthis
.
- Parameter
- UniformSample(*args, **kwargs)
Overloaded function.
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, withmixing_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 ismixing_steps
* the number of times this function is called. If asubspace
is provided, the random samples are constrained to lie in the affine subspace throughprevious_sample
, spanned by the columns ofsubspace
. 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 thesubspace
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. –
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 thatA
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 byB = A⁻¹
, whenA
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.
__init__(self: pydrake.geometry.optimization.Hyperellipsoid) -> None
Constructs a default (zero-dimensional, nonempty) set.
__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().
__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. Ifreference_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. –
__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 forradius
.
- static MakeHypersphere(radius: float, center: numpy.ndarray[numpy.float64[m, 1]]) pydrake.geometry.optimization.Hyperellipsoid
Constructs a hypersphere with
radius
andcenter
.
- 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. –
- Parameter
- 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.
__init__(self: pydrake.geometry.optimization.Hyperrectangle) -> None
Constructs a default (zero-dimensional, nonempty) hyperrectangle.
__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” ofv
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.
__init__(self: pydrake.geometry.optimization.Intersection) -> None
Constructs a default (zero-dimensional, nonempty) set.
__init__(self: pydrake.geometry.optimization.Intersection, sets: list[pydrake.geometry.optimization.ConvexSet]) -> None
Constructs the intersection from a vector of convex sets.
__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 thedomain
must describe elements in the same ambient dimension (but that dimension can be any positive integer).- Parameter
- 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 foroptions.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. –
- Parameter
- 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 ofIris
or the joint limits of the inputplant
in the case ofIrisInConfigurationSpace
. If this option is specified, IRIS regions will be confined to the intersection between the domain andbounding_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 inprog_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.
__init__(self: pydrake.geometry.optimization.MinkowskiSum) -> None
Constructs a default (zero-dimensional, nonempty) set.
__init__(self: pydrake.geometry.optimization.MinkowskiSum, sets: list[pydrake.geometry.optimization.ConvexSet]) -> None
Constructs the sum from a vector of convex sets.
__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.
__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. Ifreference_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.
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()) –
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.
__init__(self: pydrake.geometry.optimization.Point) -> None
Constructs a default (zero-dimensional, nonempty) set.
__init__(self: pydrake.geometry.optimization.Point, x: numpy.ndarray[numpy.float64[m, 1]]) -> None
Constructs a Point.
__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. Ifreference_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 variables
, 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
- 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.
__init__(self: pydrake.geometry.optimization.Spectrahedron) -> None
Default constructor (yields the zero-dimensional nonempty set).
__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.
__init__(self: pydrake.geometry.optimization.VPolytope) -> None
Constructs a set with no vertices in the zero-dimensional space, which is empty (by convention).
__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).
__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. –
__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 thefilename
.- Precondition:
ambient_dimension() == 3.