pydrake.planning
A collection of motion planning algorithms for finding configurations and/or trajectories of dynamical systems.
- pydrake.planning.AddDirectCollocationConstraint(constraint: pydrake.planning.DirectCollocationConstraint, time_step: numpy.ndarray[object[m, 1]], state: numpy.ndarray[object[m, 1]], next_state: numpy.ndarray[object[m, 1]], input: numpy.ndarray[object[m, 1]], next_input: numpy.ndarray[object[m, 1]], prog: pydrake.solvers.MathematicalProgram) pydrake.solvers.Binding[Constraint]
Helper method to add a DirectCollocationConstraint to the
prog
, ensuring that the order of variables in the binding matches the order expected by the constraint.
- class pydrake.planning.BodyShapeDescription
BodyShapeDescription captures all the information necessary to describe a SceneGraph collision shape associated with a MultibodyPlant Body: a shape S, the MultibodyPlant body B (identified by model instance and body names), and the rigid pose of the shape S relative to the body B, X_BS.
Most clients should use the factory method MakeBodyShapeDescription() to construct a valid BodyShapeDescription; it will extract and verify the correct information from a multibody plant and its context.
When moved-from, this object models a “null” description and all of the getter functions will throw.
- __init__(*args, **kwargs)
Overloaded function.
__init__(self: pydrake.planning.BodyShapeDescription, shape: pydrake.geometry.Shape, X_BS: pydrake.math.RigidTransform, model_instance_name: str, body_name: str) -> None
Constructs a description with the given attributes. Does not check or enforce correctness; callers are responsible for providing consistent input.
__init__(self: pydrake.planning.BodyShapeDescription, other: pydrake.planning.BodyShapeDescription) -> None
- body_name(self: pydrake.planning.BodyShapeDescription) str
- Returns
the body name passed at construction.
- model_instance_name(self: pydrake.planning.BodyShapeDescription) str
- Returns
the model instance name passed at construction.
- pose_in_body(self: pydrake.planning.BodyShapeDescription) pydrake.math.RigidTransform
- Returns
X_BS
: The pose passed at construction.
- Returns
- shape(self: pydrake.planning.BodyShapeDescription) pydrake.geometry.Shape
- Returns
the shape passed at construction.
- class pydrake.planning.CollisionChecker
Interface for collision checkers to use.
This interface builds on the basic multi-threading idiom of Drake: one context per thread. It offers two models to achieve multi-threaded parallel collision checking:
using thread pools (e.g. OpenMP or similar) and “implicit contexts” managed
by this object - using arbitrary threads and “explicit contexts” created by this object
<h5>Implicit Context Parallelism</h5>
Many methods of this class aren’t designed for entry from arbitrary threads (e.g. from std::async threads), but rather are designed for use with a main thread and various thread-pool-parallel operations achieved by using directives like
omp parallel
. To support this usage, the base classAllocateContexts()
protected method establishes a pool of contexts to support the implicit context parallelism specified in the constructor. (Note: if the collision checker declares that parallel checking is not supported, only one implict context will be allocated).AllocateContexts()
must be called and only be called as part of the constructor of a derived class defined as final.Once the context pool is created, clients can access a thread-associated context by using
model_context(optional<int> context_number)
and related methods. These methods may be called in two ways:without a context number, the association between thread and context uses the
OpenMP notion of thread number - with a context number, the method uses the context corresponding to the provided number
Without a context number, these context access methods are only safe under the following conditions:
the caller is the “main thread”
the caller is an OpenMP team thread during execution of a parallel region
With a context number, these context access methods are only safe under the following conditions:
no two or more threads simultaneously use the same context number
Methods supporting implicit context parallelism are noted below by having a reference to this section; as a rule of thumb, any public method that takes a
context_number
argument uses implicit context parallelism.Users of this class (derived classes and others) can write their own parallel operations using implicit contexts, provided they limit parallel blocks to only use
const
methods or methods marked to support implicit contexts parallelism, and the parallel operations are:without a context number, only with parallelism using OpenMP directives
with a context number, via a parallelization method that provides a notion of
thread numbers similar in behavior to OpenMP’s (i.e. a thread number in [0, number of threads), not arbitrary values like
std::this_thread::get_id()
)To determine the greatest implicit context parallelism that can be achieved in a parallelized operation,
GetNumberOfThreads(Parallelism parallelize)
returns the lesser of the providedparallelism
and the supported implicit context parallelism.<h5>Explicit Context Parallelism</h5>
It is also possible to use arbitrary thread models to perform collision checking in parallel using explicitly created contexts from this class. Contexts returned from MakeStandaloneModelContext() may be used in any thread, using only
const
methods of this class, or “explicit context” methods.Explicit contexts are tracked by this class using
std::weak_ptr
to track their lifetimes. This mechanism is used by PerformOperationAgainstAllModelContexts to map an operation over all collision contexts, whether explicit or implicit.Methods supporting explicit context parallelism are noted below by having a reference to this section; as a rule of thumb, any public method that takes a
model_context
first argument uses explicit context parallelism.In practice, multi-threaded collision checking with explicit contexts may look something like the example below.
Click to expand C++ code...
const Eigen::VectorXd start_q ... const Eigen::VectorXd sample_q1 ... const Eigen::VectorXd sample_q2 ... const auto check_edge_to = [&collision_checker, &start_q] ( const Eigen::VectorXd& sample_q, CollisionCheckerContext* explicit_context) { return collision_checker.CheckContextEdgeCollisionFree( explicit_context, start_q, sample_q); }; const auto context_1 = collision_checker.MakeStandaloneModelContext(); const auto context_2 = collision_checker.MakeStandaloneModelContext(); auto future_q1 = std::async(std::launch::async, check_edge_to, sample_q1, context_1.get()); auto future_q2 = std::async(std::launch::async, check_edge_to, sample_q2, context_2.get()); const double edge_1_valid = future_q1.get(); const double edge_2_valid = future_q2.get();
<h5>Mixing Threading Models</h5>
It is possible to support mixed threading models, i.e., using both OpenMP thread pools and arbitrary threads. In this case, each arbitrary thread (say, from std::async) should have its own instance of a collision checker made using Clone(). Then each arbitrary thread will have its own implicit context pool.
<h5>Implementing Derived Classes</h5>
Collision checkers deriving from CollisionChecker must support parallel operations from both of the above parallelism models. This is generally accomplished by placing all mutable state within the per-thread context. If this cannot be accomplished, the shared mutable state must be accessed in a thread-safe manner. There are APIs that depend on SupportsParallelChecking() (e.g., CheckConfigsCollisionFree(), CheckEdgeCollisionFreeParallel(), CheckEdgesCollisionFree(), etc); a derived implementation should return
False
from SupportsParallelChecking() if there is no meaningful benefit to attempting to do work in parallel (e.g., they must fully serialize on shared state).- __init__(*args, **kwargs)
- AddCollisionShape(self: pydrake.planning.CollisionChecker, group_name: str, description: pydrake.planning.BodyShapeDescription) bool
Requests the addition of a shape to a body, both given in
description
. If added, the shape will belong to the named geometry group.- Parameter
group_name
: The name of the group to add the geometry to.
- Parameter
description
: The data describing the shape and target body.
- Returns
True
if the shape was added.
- Parameter
- AddCollisionShapes(*args, **kwargs)
Overloaded function.
AddCollisionShapes(self: pydrake.planning.CollisionChecker, group_name: str, descriptions: list[pydrake.planning.BodyShapeDescription]) -> int
Requests the addition of N shapes to N bodies, each given in the set of
descriptions
. Each added shape will belong to the named geometry group.- Parameter
group_name
: The name of the group to add the geometry to.
- Parameter
descriptions
: The descriptions of N (shape, body) pairs.
- Returns
The total number of shapes in
descriptions
that got added.
AddCollisionShapes(self: pydrake.planning.CollisionChecker, geometry_groups: dict[str, list[pydrake.planning.BodyShapeDescription]]) -> dict[str, int]
Requests the addition of a collection of shapes to bodies across multiple geometry groups.
geometry_groups
specifies a collection of (shape, body) descriptors across multiple geometry groups.- Parameter
geometry_groups
: A map from a named geometry group to the (shape, body) pairs to add to that group.
- Returns
A map from input named geometry group to the number of geometries added to that group.
- AddCollisionShapeToBody(self: pydrake.planning.CollisionChecker, group_name: str, bodyA: pydrake.multibody.tree.RigidBody, shape: pydrake.geometry.Shape, X_AG: pydrake.math.RigidTransform) bool
Requests the addition of
shape
to the body A in the checker’s model The addedshape
will belong to the named geometry group.- Parameter
group_name
: The name of the group to add the geometry to.
- Parameter
bodyA
: The body the shape should be rigidly affixed to.
- Parameter
shape
: The requested shape, defined in its canonical frame G.
- Parameter
X_AG
: The pose of the shape in body A’s frame.
- Returns
True
if the shape was added.
- Parameter
- AddCollisionShapeToFrame(self: pydrake.planning.CollisionChecker, group_name: str, frameA: pydrake.multibody.tree.Frame, shape: pydrake.geometry.Shape, X_AG: pydrake.math.RigidTransform) bool
Requests the addition of
shape
to the frame A in the checker’s model. The addedshape
will belong to the named geometry group.- Parameter
group_name
: The name of the group to add the geometry to.
- Parameter
frameA
: The frame the shape should be rigidly affixed to.
- Parameter
shape
: The requested shape, defined in its canonical frame G.
- Parameter
X_AG
: The pose of the shape in the frame A.
- Returns
True
if the shape was added.
- Parameter
- CalcContextRobotClearance(self: pydrake.planning.CollisionChecker, model_context: pydrake.planning.CollisionCheckerContext, q: numpy.ndarray[numpy.float64[m, 1]], influence_distance: float) pydrake.planning.RobotClearance
Explicit Context-based version of CalcRobotClearance().
- Raises
RuntimeError if model_context is nullptr. –
See also
ccb_explicit_contexts “Explicit Context Parallelism”.
- CalcRobotClearance(self: pydrake.planning.CollisionChecker, q: numpy.ndarray[numpy.float64[m, 1]], influence_distance: float, context_number: Optional[int] = None) pydrake.planning.RobotClearance
Calculates the distance, ϕ, and distance Jacobian, Jqᵣ_ϕ, for each potential collision whose distance is less than
influence_distance
, using the current thread’s associated context.Distances for filtered collisions will not be returned.
Distances between a pair of robot bodies (i.e., where
collision_types()
reportsSelfCollision
) report one body’s index inrobot_indices()
and the the other body’s inother_indices()
; which body appears in which column is arbitrary.The total number of rows can depend on how the model is defined and how a particular CollisionChecker instance is implemented (see MaxNumDistances()).
See also
RobotClearance for details on the quantities ϕ and Jqᵣ_ϕ (and other details).
- Parameter
context_number
: Optional implicit context number.
See also
ccb_implicit_contexts “Implicit Context Parallelism”.
- Parameter
- CheckConfigCollisionFree(self: pydrake.planning.CollisionChecker, q: numpy.ndarray[numpy.float64[m, 1]], context_number: Optional[int] = None) bool
Checks a single configuration for collision using the current thread’s associated context.
- Parameter
q
: Configuration to check
- Parameter
context_number
: Optional implicit context number.
- Returns
true if collision free, false if in collision.
See also
ccb_implicit_contexts “Implicit Context Parallelism”.
- Parameter
- CheckConfigsCollisionFree(self: pydrake.planning.CollisionChecker, configs: list[numpy.ndarray[numpy.float64[m, 1]]], parallelize: pydrake.common.Parallelism = True) list[int]
Checks a vector of configurations for collision, evaluating in parallel when supported and enabled by
parallelize
. Parallelization in configuration collision checks is provided using OpenMP and is supported when both: (1) the collision checker declares that parallelization is supported (i.e. when SupportsParallelChecking() is true) and (2) when multiple OpenMP threads are available for execution. See collision_checker_parallel_edge “function-level parallelism” for guidance on proper usage.- Parameter
configs
: Configurations to check
- Parameter
parallelize
: How much should collision checks be parallelized?
- Returns
std::vector<uint8_t>, one for each configuration in configs. For each configuration, 1 if collision free, 0 if in collision.
- Parameter
- CheckContextConfigCollisionFree(self: pydrake.planning.CollisionChecker, model_context: pydrake.planning.CollisionCheckerContext, q: numpy.ndarray[numpy.float64[m, 1]]) bool
Explicit Context-based version of CheckConfigCollisionFree().
- Raises
RuntimeError if model_context is nullptr. –
See also
ccb_explicit_contexts “Explicit Context Parallelism”.
- CheckContextEdgeCollisionFree(self: pydrake.planning.CollisionChecker, model_context: pydrake.planning.CollisionCheckerContext, q1: numpy.ndarray[numpy.float64[m, 1]], q2: numpy.ndarray[numpy.float64[m, 1]]) bool
- CheckEdgeCollisionFree(self: pydrake.planning.CollisionChecker, q1: numpy.ndarray[numpy.float64[m, 1]], q2: numpy.ndarray[numpy.float64[m, 1]], context_number: Optional[int] = None) bool
Checks a single configuration-to-configuration edge for collision, using the current thread’s associated context.
- Parameter
q1
: Start configuration for edge.
- Parameter
q2
: End configuration for edge.
- Parameter
context_number
: Optional implicit context number.
- Returns
true if collision free, false if in collision.
See also
ccb_implicit_contexts “Implicit Context Parallelism”.
- Parameter
- CheckEdgeCollisionFreeParallel(self: pydrake.planning.CollisionChecker, q1: numpy.ndarray[numpy.float64[m, 1]], q2: numpy.ndarray[numpy.float64[m, 1]], parallelize: pydrake.common.Parallelism = True) bool
Checks a single configuration-to-configuration edge for collision. Collision check is parallelized via OpenMP when supported. See collision_checker_parallel_edge “function-level parallelism” for guidance on proper usage.
- Parameter
q1
: Start configuration for edge.
- Parameter
q2
: End configuration for edge.
- Parameter
parallelize
: How much should edge collision check be parallelized?
- Returns
true if collision free, false if in collision.
- Parameter
- CheckEdgesCollisionFree(self: pydrake.planning.CollisionChecker, edges: list[tuple[numpy.ndarray[numpy.float64[m, 1]], numpy.ndarray[numpy.float64[m, 1]]]], parallelize: pydrake.common.Parallelism = True) list[int]
Checks multiple configuration-to-configuration edges for collision. Collision checks are parallelized via OpenMP when supported and enabled by
parallelize
. See collision_checker_parallel_edge “function-level parallelism” for guidance on proper usage.- Parameter
edges
: Edges to check, each in the form of pair<q1, q2>.
- Parameter
parallelize
: How much should edge collision checks be parallelized?
- Returns
std::vector<uint8_t>, one for each edge in edges. For each edge, 1 if collision free, 0 if in collision.
- Parameter
- ClassifyBodyCollisions(self: pydrake.planning.CollisionChecker, q: numpy.ndarray[numpy.float64[m, 1]], context_number: Optional[int] = None) list[pydrake.planning.RobotCollisionType]
Classifies which robot bodies are in collision (and which type of collision) for the provided configuration
q
, using the current thread’s associated context.- Parameter
context_number
: Optional implicit context number.
- Returns
a vector of collision types arranged in body index order. Only entries for robot bodies are guaranteed to be valid; entries for environment bodies are populated with kNoCollision, regardless of their actual status.
See also
ccb_implicit_contexts “Implicit Context Parallelism”.
- Parameter
- ClassifyContextBodyCollisions(self: pydrake.planning.CollisionChecker, model_context: pydrake.planning.CollisionCheckerContext, q: numpy.ndarray[numpy.float64[m, 1]]) list[pydrake.planning.RobotCollisionType]
Explicit Context-based version of ClassifyBodyCollisions().
- Raises
RuntimeError if model_context is nullptr. –
See also
ccb_explicit_contexts “Explicit Context Parallelism”.
- ComputeConfigurationDistance(self: pydrake.planning.CollisionChecker, q1: numpy.ndarray[numpy.float64[m, 1]], q2: numpy.ndarray[numpy.float64[m, 1]]) float
Computes configuration-space distance between the provided configurations
q1
andq2
, using the distance function configured at construction- time or via SetConfigurationDistanceFunction().
- distance_and_interpolation_provider(self: pydrake.planning.CollisionChecker) pydrake.planning.DistanceAndInterpolationProvider
Gets the DistanceAndInterpolationProvider in use.
- edge_step_size(self: pydrake.planning.CollisionChecker) float
Gets the current edge step size.
- get_body(self: pydrake.planning.CollisionChecker, body_index: pydrake.multibody.tree.BodyIndex) pydrake.multibody.tree.RigidBody
- Returns
a
const
body reference to a body in the full model’s plant for the givenbody_index
.
- GetAllAddedCollisionShapes(self: pydrake.planning.CollisionChecker) dict[str, list[pydrake.planning.BodyShapeDescription]]
Gets all checker geometries currently added across the whole checker.
- Returns
A mapping from each geometry group name to the collection of (shape, body) descriptions in that group.
- GetFilteredCollisionMatrix(self: pydrake.planning.CollisionChecker) numpy.ndarray[numpy.int32[m, n]]
Gets the “active” collision filter matrix.
- GetLargestPadding(self: pydrake.planning.CollisionChecker) float
Gets the current largest collision padding across all (robot, ) body pairs. This excludes the meaningless zeros on the diagonal and environment-environment pairs; the return value *can be negative.
- GetNominalFilteredCollisionMatrix(self: pydrake.planning.CollisionChecker) numpy.ndarray[numpy.int32[m, n]]
Returns the “nominal” collision filter matrix. The nominal matrix is initialized at construction time and represents the configuration of the model’s plant and scene graph. It serves as a reference point to assess any changes to collision filters beyond this checker’s intrinsic model.
Collisions between bodies A and B are filtered in the following cases:
There exists a welded path between A and B.
SceneGraph has filtered the collisions between all pairs of geometries
of A and B.
Note: SceneGraph allows arbitrary collision filter configuration at the geometry* level. The filters on one geometry of body need not be the same as another geometry on the same body. CollisionChecker is body centric. It requires all geometries on a body to be filtered homogeneously. A SceneGraph that violates this stricter requirement cannot be used in a CollisionChecker. It is highly unlikely that a SceneGraph instance will ever be in this configuration by accident.
- GetPaddingBetween(*args, **kwargs)
Overloaded function.
GetPaddingBetween(self: pydrake.planning.CollisionChecker, bodyA_index: pydrake.multibody.tree.BodyIndex, bodyB_index: pydrake.multibody.tree.BodyIndex) -> float
Gets the padding value for the pair of bodies specified. If the body indices are the same, zero will always be returned.
- Raises
RuntimeError if either body index is out of range. –
GetPaddingBetween(self: pydrake.planning.CollisionChecker, bodyA: pydrake.multibody.tree.RigidBody, bodyB: pydrake.multibody.tree.RigidBody) -> float
Overload that uses body references.
- GetPaddingMatrix(self: pydrake.planning.CollisionChecker) numpy.ndarray[numpy.float64[m, n]]
Gets the collision padding matrix.
- GetZeroConfiguration(self: pydrake.planning.CollisionChecker) numpy.ndarray[numpy.float64[m, 1]]
- Returns
a generalized position vector, sized according to the full model, whose values are all zero.
Warning
A zero vector is not necessarily a valid configuration, e.g., in case the configuration has quaternions, or position constraints, or etc.
- InterpolateBetweenConfigurations(self: pydrake.planning.CollisionChecker, q1: numpy.ndarray[numpy.float64[m, 1]], q2: numpy.ndarray[numpy.float64[m, 1]], ratio: float) numpy.ndarray[numpy.float64[m, 1]]
Interpolates between provided configurations
q1
andq2
.- Parameter
ratio
: Interpolation ratio.
- Returns
Interpolated configuration.
- Raises
RuntimeError if ratio is not in range [0, 1] –
See also
ConfigurationInterpolationFunction for more.
- Parameter
- IsCollisionFilteredBetween(*args, **kwargs)
Overloaded function.
IsCollisionFilteredBetween(self: pydrake.planning.CollisionChecker, bodyA_index: pydrake.multibody.tree.BodyIndex, bodyB_index: pydrake.multibody.tree.BodyIndex) -> bool
Checks if collision is filtered between the two bodies specified. Note: collision between two environment bodies is always filtered.
- Raises
RuntimeError if either body index is out of range. –
IsCollisionFilteredBetween(self: pydrake.planning.CollisionChecker, bodyA: pydrake.multibody.tree.RigidBody, bodyB: pydrake.multibody.tree.RigidBody) -> bool
Overload that uses body references.
- IsPartOfRobot(*args, **kwargs)
Overloaded function.
IsPartOfRobot(self: pydrake.planning.CollisionChecker, body: pydrake.multibody.tree.RigidBody) -> bool
- Returns
true if the indicated body is part of the robot.
IsPartOfRobot(self: pydrake.planning.CollisionChecker, body_index: pydrake.multibody.tree.BodyIndex) -> bool
- Returns
true if the indicated body is part of the robot.
- MakeStandaloneConfigurationDistanceFunction(self: pydrake.planning.CollisionChecker) Callable[[numpy.ndarray[numpy.float64[m, 1]], numpy.ndarray[numpy.float64[m, 1]]], float]
- Returns
a functor that captures this object, so it can be used like a free function. The returned functor is only valid during the lifetime of this object. The math of the function is equivalent to ComputeConfigurationDistance().
Warning
do not pass this standalone function back into SetConfigurationDistanceFunction() function; doing so would create an infinite loop.
- MakeStandaloneConfigurationInterpolationFunction(self: pydrake.planning.CollisionChecker) Callable[[numpy.ndarray[numpy.float64[m, 1]], numpy.ndarray[numpy.float64[m, 1]], float], numpy.ndarray[numpy.float64[m, 1]]]
- Returns
a functor that captures this object, so it can be used like a free function. The returned functor is only valid during the lifetime of this object. The math of the function is equivalent to InterpolateBetweenConfigurations().
Warning
do not pass this standalone function back into our SetConfigurationInterpolationFunction() function; doing so would create an infinite loop.
- MakeStandaloneModelContext(self: pydrake.planning.CollisionChecker) pydrake.planning.CollisionCheckerContext
Make and track a CollisionCheckerContext. The returned context will participate in PerformOperationAgainstAllModelContexts() until it is destroyed.
- MaxContextNumDistances(self: pydrake.planning.CollisionChecker, model_context: pydrake.planning.CollisionCheckerContext) int
Explicit Context-based version of MaxNumDistances().
See also
ccb_explicit_contexts “Explicit Context Parallelism”.
- MaxNumDistances(self: pydrake.planning.CollisionChecker, context_number: Optional[int] = None) int
Returns an upper bound on the number of distances returned by CalcRobotClearance(), using the current thread’s associated context.
- Parameter
context_number
: Optional implicit context number.
See also
ccb_implicit_contexts “Implicit Context Parallelism”.
- Parameter
- MaybeGetUniformRobotEnvironmentPadding(self: pydrake.planning.CollisionChecker) Optional[float]
If the padding between all robot bodies and environment bodies is the same, returns the common padding value. Returns nullopt otherwise.
- MaybeGetUniformRobotRobotPadding(self: pydrake.planning.CollisionChecker) Optional[float]
If the padding between all pairs of robot bodies is the same, returns the common padding value. Returns nullopt otherwise.
- MeasureContextEdgeCollisionFree(self: pydrake.planning.CollisionChecker, model_context: pydrake.planning.CollisionCheckerContext, q1: numpy.ndarray[numpy.float64[m, 1]], q2: numpy.ndarray[numpy.float64[m, 1]]) pydrake.planning.EdgeMeasure
Explicit Context-based version of MeasureEdgeCollisionFree().
- Raises
RuntimeError if model_context is nullptr. –
See also
ccb_explicit_contexts “Explicit Context Parallelism”.
- MeasureEdgeCollisionFree(self: pydrake.planning.CollisionChecker, q1: numpy.ndarray[numpy.float64[m, 1]], q2: numpy.ndarray[numpy.float64[m, 1]], context_number: Optional[int] = None) pydrake.planning.EdgeMeasure
Checks a single configuration-to-configuration edge for collision, using the current thread’s associated context.
- Parameter
q1
: Start configuration for edge.
- Parameter
q2
: End configuration for edge.
- Parameter
context_number
: Optional implicit context number.
- Returns
A measure of how much of the edge is collision free.
See also
ccb_implicit_contexts “Implicit Context Parallelism”.
- Parameter
- MeasureEdgeCollisionFreeParallel(self: pydrake.planning.CollisionChecker, q1: numpy.ndarray[numpy.float64[m, 1]], q2: numpy.ndarray[numpy.float64[m, 1]], parallelize: pydrake.common.Parallelism = True) pydrake.planning.EdgeMeasure
Checks a single configuration-to-configuration edge for collision. Collision check is parallelized via OpenMP when supported. See collision_checker_parallel_edge “function-level parallelism” for guidance on proper usage.
- Parameter
q1
: Start configuration for edge.
- Parameter
q2
: End configuration for edge.
- Parameter
parallelize
: How much should edge collision check be parallelized?
- Returns
A measure of how much of the edge is collision free.
- Parameter
- MeasureEdgesCollisionFree(self: pydrake.planning.CollisionChecker, edges: list[tuple[numpy.ndarray[numpy.float64[m, 1]], numpy.ndarray[numpy.float64[m, 1]]]], parallelize: pydrake.common.Parallelism = True) list[pydrake.planning.EdgeMeasure]
Checks multiple configuration-to-configuration edge for collision. Collision checks are parallelized via OpenMP when supported and enabled by
parallelize
. See collision_checker_parallel_edge “function-level parallelism” for guidance on proper usage.- Parameter
edges
: Edges to check, each in the form of pair<q1, q2>.
- Parameter
parallelize
: How much should edge collision checks be parallelized?
- Returns
A measure of how much of each edge is collision free. The iᵗʰ entry is the result for the iᵗʰ edge.
- Parameter
- model(self: pydrake.planning.CollisionChecker) pydrake.planning.RobotDiagram
- Returns
a
const
reference to the full model.
- model_context(self: pydrake.planning.CollisionChecker, context_number: Optional[int] = None) pydrake.planning.CollisionCheckerContext
Accesses a collision checking context from within the implicit context pool owned by this collision checker.
- Parameter
context_number
: Optional implicit context number.
- Returns
a
const
reference to either the collision checking context given by thecontext_number
, or when nullopt the context to be used with the current OpenMP thread.
See also
ccb_implicit_contexts “Implicit Context Parallelism”.
- Parameter
- num_allocated_contexts(self: pydrake.planning.CollisionChecker) int
- Returns
the number of internal (not standalone) per-thread contexts.
- PerformOperationAgainstAllModelContexts(self: pydrake.planning.CollisionChecker, operation: Callable[[pydrake.planning.RobotDiagram, pydrake.planning.CollisionCheckerContext], None]) None
Allows externally-provided operations that must be performed against all contexts in the per-thread context pool, and any standalone contexts made with MakeStandaloneModelContext().
For any standalone contexts, note that it is illegal to mutate a context from two different threads. No other threads should be mutating any of our standalone contexts when this function is called.
- plant(self: pydrake.planning.CollisionChecker) pydrake.multibody.plant.MultibodyPlant
- Returns
a `const reference to the full model’s plant.
- plant_context(self: pydrake.planning.CollisionChecker, context_number: Optional[int] = None) pydrake.systems.framework.Context
Accesses a multibody plant sub-context context from within the implicit context pool owned by this collision checker.
- Parameter
context_number
: Optional implicit context number.
- Returns
a
const
reference to the multibody plant sub-context within the context given by thecontext_number
, or when nullopt the context to be used with the current OpenMP thread.
See also
ccb_implicit_contexts “Implicit Context Parallelism”.
- Parameter
- RemoveAllAddedCollisionShapes(*args, **kwargs)
Overloaded function.
RemoveAllAddedCollisionShapes(self: pydrake.planning.CollisionChecker, group_name: str) -> None
Removes all added checker geometries which belong to the named group.
RemoveAllAddedCollisionShapes(self: pydrake.planning.CollisionChecker) -> None
Removes all added checker geometries from all geometry groups.
- robot_model_instances(self: pydrake.planning.CollisionChecker) list[pydrake.multibody.tree.ModelInstanceIndex]
Gets the set of model instances belonging to the robot. The returned vector has no duplicates and is in sorted order.
- set_edge_step_size(self: pydrake.planning.CollisionChecker, edge_step_size: float) None
Sets the edge step size to
edge_step_size
.- Raises
RuntimeError if edge_step_size is not positive. –
- SetCollisionFilteredBetween(*args, **kwargs)
Overloaded function.
SetCollisionFilteredBetween(self: pydrake.planning.CollisionChecker, bodyA_index: pydrake.multibody.tree.BodyIndex, bodyB_index: pydrake.multibody.tree.BodyIndex, filter_collision: bool) -> None
Declares the body pair (bodyA, bodyB) to be filtered (or not) based on
filter_collision
.- Parameter
filter_collision
: Sets the to body pair to be filtered if
True
.
- Raises
RuntimeError if either body index is out of range. –
RuntimeError if both indices refer to the same body. –
RuntimeError if both indices refer to environment bodies. –
SetCollisionFilteredBetween(self: pydrake.planning.CollisionChecker, bodyA: pydrake.multibody.tree.RigidBody, bodyB: pydrake.multibody.tree.RigidBody, filter_collision: bool) -> None
Overload that uses body references.
- SetCollisionFilteredWithAllBodies(*args, **kwargs)
Overloaded function.
SetCollisionFilteredWithAllBodies(self: pydrake.planning.CollisionChecker, body_index: pydrake.multibody.tree.BodyIndex) -> None
Declares that body pair (B, O) is filtered (for all bodies O in this checker’s plant).
- Raises
RuntimeError if body_index is out of range. –
RuntimeError if body_index refers to an environment body. –
SetCollisionFilteredWithAllBodies(self: pydrake.planning.CollisionChecker, body: pydrake.multibody.tree.RigidBody) -> None
Overload that uses body references.
- SetCollisionFilterMatrix(self: pydrake.planning.CollisionChecker, filter_matrix: numpy.ndarray[numpy.int32[m, n]]) None
Sets the “active” collision filter matrix
- Parameter
filter_matrix
: must meet the above conditions to be a “consistent” collision filter matrix.
- Raises
RuntimeError if the given matrix is incompatible with this –
collision checker, or if it is inconsistent. –
- Parameter
- SetConfigurationDistanceFunction(self: pydrake.planning.CollisionChecker, distance_function: Callable[[numpy.ndarray[numpy.float64[m, 1]], numpy.ndarray[numpy.float64[m, 1]]], float]) None
Sets the configuration distance function to
distance_function
.- Precondition:
distance_function satisfies the requirements documented on ConfigurationDistanceFunction and a DistanceAndInterpolationProvider is not already in use.
- Precondition:
the collision checker was created with separate distance and interpolation functions, not a combined DistanceAndInterpolationProvider.
Note
the
distance_function
object will be copied and retained by this collision checker, so if the function has any lambda-captured data then that data must outlive this collision checker.
- SetConfigurationInterpolationFunction(self: pydrake.planning.CollisionChecker, interpolation_function: Callable[[numpy.ndarray[numpy.float64[m, 1]], numpy.ndarray[numpy.float64[m, 1]], float], numpy.ndarray[numpy.float64[m, 1]]]) None
Sets the configuration interpolation function to
interpolation_function
.- Parameter
interpolation_function
: a functor, or nullptr. If nullptr, the default function will be configured and used.
- Precondition:
interpolation_function satisfies the requirements documented on ConfigurationInterpolationFunction, or is nullptr and a DistanceAndInterpolationProvider is not already in use.
- Precondition:
the collision checker was created with separate distance and interpolation functions, not a combined DistanceAndInterpolationProvider.
Note
the
interpolation_function
object will be copied and retained by this collision checker, so if the function has any lambda-captured data then that data must outlive this collision checker.Note
the default function uses linear interpolation for most variables, and uses slerp for quaternion valued variables.
- Parameter
- SetDistanceAndInterpolationProvider(self: pydrake.planning.CollisionChecker, provider: pydrake.planning.DistanceAndInterpolationProvider) None
Sets the distance and interpolation provider to use. Note that in case any of the (to-be-deprecated) separate distance and interpolation functions were in use, this supplants both of them.
- Precondition:
provider satisfies the requirements documents on DistanceAndInterpolationProvider.
- SetPaddingAllRobotEnvironmentPairs(self: pydrake.planning.CollisionChecker, padding: float) None
Sets the padding for all (robot, environment) pairs.
- Raises
RuntimeError if the collision_checker_padding_prereqs –
"configuration prerequisites" are not met. –
- SetPaddingAllRobotRobotPairs(self: pydrake.planning.CollisionChecker, padding: float) None
Sets the padding for all (robot, robot) pairs.
- Raises
RuntimeError if the collision_checker_padding_prereqs –
"configuration prerequisites" are not met. –
- SetPaddingBetween(*args, **kwargs)
Overloaded function.
SetPaddingBetween(self: pydrake.planning.CollisionChecker, bodyA_index: pydrake.multibody.tree.BodyIndex, bodyB_index: pydrake.multibody.tree.BodyIndex, padding: float) -> None
Sets the padding value for the pair of bodies specified.
- Raises
RuntimeError if the collision_checker_padding_prereqs –
"configuration prerequisites" are not met or ``bodyA_index == –
bodyB_index``. –
SetPaddingBetween(self: pydrake.planning.CollisionChecker, bodyA: pydrake.multibody.tree.RigidBody, bodyB: pydrake.multibody.tree.RigidBody, padding: float) -> None
Overload that uses body references.
- SetPaddingMatrix(self: pydrake.planning.CollisionChecker, collision_padding: numpy.ndarray[numpy.float64[m, n]]) None
Sets the collision padding matrix. Note that this matrix contains all padding data, both robot-robot “self” padding, and robot-environment padding.
collision_padding
must have the following properties to be considered valid.It is a square NxN matrix (where N is the total number of bodies).
Diagonal values are all zero.
Entries involving only environment bodies are all zero.
It is symmetric.
All values are finite.
- Raises
RuntimeError if collision_padding doesn't have the enumerated –
properties. –
- SetPaddingOneRobotBodyAllEnvironmentPairs(self: pydrake.planning.CollisionChecker, body_index: pydrake.multibody.tree.BodyIndex, padding: float) None
Sets the environment collision padding for the provided robot body with respect to all environment bodies.
- Raises
RuntimeError if the collision_checker_padding_prereqs –
"configuration prerequisites" are not met. –
- SupportsParallelChecking(self: pydrake.planning.CollisionChecker) bool
Does the collision checker support true parallel collision checks?
- Returns
true if parallel checking is supported.
- UpdateContextPositions(self: pydrake.planning.CollisionChecker, model_context: pydrake.planning.CollisionCheckerContext, q: numpy.ndarray[numpy.float64[m, 1]]) pydrake.systems.framework.Context
Explicit Context-based version of UpdatePositions().
- Raises
RuntimeError if model_context is nullptr. –
See also
ccb_explicit_contexts “Explicit Context Parallelism”.
- UpdatePositions(self: pydrake.planning.CollisionChecker, q: numpy.ndarray[numpy.float64[m, 1]], context_number: Optional[int] = None) pydrake.systems.framework.Context
Updates the generalized positions
q
in the implicit context specified and returns a reference to the MultibodyPlant’s now-updated context. The implicit context is either that specified bycontext_number
, or when nullopt the context to be used with the current OpenMP thread.- Parameter
context_number
: Optional implicit context number.
See also
ccb_implicit_contexts “Implicit Context Parallelism”.
- Parameter
- class pydrake.planning.CollisionCheckerContext
This class represents the data necessary for CollisionChecker to operate safely across multiple threads in its
const
API. Instances of this class are owned and managed by a particular CollisionChecker.If using OMP to perform parallel const queries on a CollisionChecker, it will never be necessary to interact with CollisionCheckerContext instances. Only if using some other threading paradigm will it be necessary to work with “stand alone” instances. See CollisionChecker’s documentation for more details.
In all cases, modifying context should happen through CollisionChecker::PerformOperationAgainstAllModelContexts(). Modifying the contained Drake Contexts directly is generally erroneous.
- __init__(self: pydrake.planning.CollisionCheckerContext, model: pydrake.planning.RobotDiagram) None
The resulting object stores an alias to
model
; the passed model should have a lifetime greater than the constructed object.- Precondition:
model is not null.
- GetQueryObject(self: pydrake.planning.CollisionCheckerContext) pydrake.geometry.QueryObject
Gets the scene graph geometry query object.
- model_context(self: pydrake.planning.CollisionCheckerContext) pydrake.systems.framework.Context
Gets the contained model context.
- plant_context(self: pydrake.planning.CollisionCheckerContext) pydrake.systems.framework.Context
Gets the contained plant context.
- scene_graph_context(self: pydrake.planning.CollisionCheckerContext) pydrake.systems.framework.Context
Gets the contained scene graph context.
- class pydrake.planning.CollisionCheckerParams
A set of common constructor parameters for a CollisionChecker. Not all subclasses of CollisionChecker will necessarily support this configuration struct, but many do so.
- __init__(*args, **kwargs)
Overloaded function.
__init__(self: pydrake.planning.CollisionCheckerParams) -> None
__init__(self: pydrake.planning.CollisionCheckerParams, **kwargs) -> None
- property configuration_distance_function
Configuration (probably weighted) distance function.
Note
Either a DistanceAndInterpolationProvider OR a ConfigurationDistanceFunction may be provided, not both. If neither is provided, a LinearDistanceAndInterpolationProvider with default weights is used.
Note
the
configuration_distance_function
object will be copied and retained by a collision checker, so if the function has any lambda-captured data then that data must outlive the collision checker.
- property distance_and_interpolation_provider
A DistanceAndInterpolationProvider to support configuration distance and interpolation operations.
Note
Either a DistanceAndInterpolationProvider OR a ConfigurationDistanceFunction may be provided, not both. If neither is provided, a LinearDistanceAndInterpolationProvider with default weights is used.
- property edge_step_size
Step size for edge checking; in units compatible with the configuration distance function. Collision checking of edges q1->q2 is performed by interpolating from q1 to q2 at edge_step_size steps and checking the interpolated configuration for collision. The value must be positive.
- property env_collision_padding
Additional padding to apply to all robot-environment collision queries. If distance between robot and environment is less than padding, the checker reports a collision.
- property implicit_context_parallelism
Specify how many contexts should be allocated to support collision checker implicit context parallelism. Defaults to the maximum parallelism. If the specific collision checker type in use declares that it does not support parallel queries, then implicit context parallelism is set to None().
See also
ccb_implicit_contexts “Implicit Context Parallelism”.
- property model
A RobotDiagram model of the robot and environment. Must not be nullptr.
- property robot_model_instances
A vector of model instance indices that identify which model instances belong to the robot. The list must be non-empty and must not include the world model instance.
- property self_collision_padding
Additional padding to apply to all robot-robot self collision queries. If distance between robot and itself is less than padding, the checker reports a collision.
- class pydrake.planning.DirectCollocation
Bases:
pydrake.planning.MultipleShooting
DirectCollocation implements the approach to trajectory optimization as described in C. R. Hargraves and S. W. Paris. Direct trajectory optimization using nonlinear programming and collocation. J Guidance, 10(4):338-342, July-August 1987. It assumes a first-order hold on the input trajectory and a cubic spline representation of the state trajectory, and adds dynamic constraints (and running costs) to the midpoints as well as the breakpoints in order to achieve a 3rd order integration accuracy.
Note: This algorithm only works with the continuous states of a system.
- __init__(self: pydrake.planning.DirectCollocation, system: pydrake.systems.framework.System, context: pydrake.systems.framework.Context, num_time_samples: int, minimum_time_step: float, maximum_time_step: float, input_port_index: Union[pydrake.systems.framework.InputPortSelection, pydrake.systems.framework.InputPortIndex] = <InputPortSelection.kUseFirstInputIfItExists: -2>, assume_non_continuous_states_are_fixed: bool = False, prog: pydrake.solvers.MathematicalProgram = None) None
Constructs the MathematicalProgram% and adds the collocation constraints.
- Parameter
system
: A dynamical system to be used in the dynamic constraints. This system must support System::ToAutoDiffXd. Note that this is aliased for the lifetime of this object.
- Parameter
context
: Required to describe any parameters of the system. The values of the state in this context do not have any effect. This context will also be “cloned” by the optimization; changes to the context after calling this method will NOT impact the trajectory optimization.
- Parameter
num_time_samples
: The number of breakpoints in the trajectory.
- Parameter
minimum_time_step
: Minimum spacing between sample times.
- Parameter
maximum_time_step
: Maximum spacing between sample times.
- Parameter
input_port_index
: A valid input port index for
system
or InputPortSelection. All other inputs on the system will be left disconnected (if they are disconnected incontext)
or will be fixed to their current values (if they are connected/fixed incontext)
.- Default: kUseFirstInputIfItExists.
$Parameter
assume_non_continuous_states_are_fixed
:
Boolean which, if true, allows this algorithm to optimize without considering the dynamics of any non-continuous states. This is helpful for optimizing systems that might have some additional book-keeping variables in their state. Only use this if you are sure that the dynamics of the additional state variables cannot impact the dynamics of the continuous states. $*Default:* false.
- Parameter
prog
: (optional). If non-null, then additional decision variables, costs, and constraints will be added into the existing MathematicalProgram. This can be useful for, e.g., combining multiple trajectory optimizations into a single program, coupled by a few constraints. If nullptr, then a new MathematicalProgram will be allocated.
- Raises
RuntimeError if system is not supported by this direct –
collocation method. –
- Parameter
- class pydrake.planning.DirectCollocationConstraint
Bases:
pydrake.solvers.Constraint
Implements the direct collocation constraints for a first-order hold on the input and a cubic polynomial representation of the state trajectories.
Note that the DirectCollocation implementation allocates only ONE of these constraints, but binds that constraint multiple times (with different decision variables, along the trajectory).
- __init__(self: pydrake.planning.DirectCollocationConstraint, system: pydrake.systems.framework.System, context: pydrake.systems.framework.Context, input_port_index: Union[pydrake.systems.framework.InputPortSelection, pydrake.systems.framework.InputPortIndex] = <InputPortSelection.kUseFirstInputIfItExists: -2>, assume_non_continuous_states_are_fixed: bool = False) None
See also
DirectCollocation constructor for a description of the parameters.
- Raises
RuntimeError if system is not supported by this direct –
collocation method. –
- class pydrake.planning.DirectTranscription
Bases:
pydrake.planning.MultipleShooting
DirectTranscription is perhaps the simplest implementation of a multiple shooting method, where we have decision variables representing the control and input at every sample time in the trajectory, and one-step of numerical integration provides the dynamic constraints between those decision variables.
- __init__(*args, **kwargs)
Overloaded function.
__init__(self: pydrake.planning.DirectTranscription, system: pydrake.systems.framework.System, context: pydrake.systems.framework.Context, num_time_samples: int, input_port_index: Union[pydrake.systems.framework.InputPortSelection, pydrake.systems.framework.InputPortIndex] = <InputPortSelection.kUseFirstInputIfItExists: -2>) -> None
Constructs the MathematicalProgram and adds the dynamic constraints. This version of the constructor is only for simple discrete-time systems (with a single periodic time step update). Continuous-time systems must call one of the constructors that takes bounds on the time step as an argument.
- Parameter
system
: A dynamical system to be used in the dynamic constraints. This system must support System::ToAutoDiffXd. Note that this is aliased for the lifetime of this object.
- Parameter
context
: Required to describe any parameters of the system. The values of the state in this context do not have any effect. This context will also be “cloned” by the optimization; changes to the context after calling this method will NOT impact the trajectory optimization.
- Parameter
num_time_samples
: The number of breakpoints in the trajectory.
- Parameter
input_port_index
: A valid input port index or valid InputPortSelection for
system
. All other inputs on the system will be left disconnected (if they are disconnected incontext)
or will be set to their current values (if they are connected/fixed incontext)
.- Default: kUseFirstInputIfItExists.
$Raises:
RuntimeError if
context.has_only_discrete_state() == false
.__init__(self: pydrake.planning.DirectTranscription, system: pydrake.systems.framework.System, context: pydrake.systems.framework.Context, num_time_samples: int, fixed_time_step: pydrake.planning.DirectTranscription.TimeStep, input_port_index: Union[pydrake.systems.framework.InputPortSelection, pydrake.systems.framework.InputPortIndex] = <InputPortSelection.kUseFirstInputIfItExists: -2>) -> None
Constructs the MathematicalProgram and adds the dynamic constraints. This version of the constructor is only for continuous-time systems; the dynamics constraints use explicit forward Euler integration.
- Parameter
system
: A dynamical system to be used in the dynamic constraints. This system must support System::ToAutoDiffXd. Note that this is aliased for the lifetime of this object.
- Parameter
context
: Required to describe any parameters of the system. The values of the state in this context do not have any effect. This context will also be “cloned” by the optimization; changes to the context after calling this method will NOT impact the trajectory optimization.
- Parameter
num_time_samples
: The number of breakpoints in the trajectory.
- Parameter
fixed_time_step
: The spacing between sample times.
- Parameter
input_port_index
: A valid input port index or valid InputPortSelection for
system
. All other inputs on the system will be left disconnected (if they are disconnected incontext)
or will be set to their current values (if they are connected/fixed incontext)
.- Default: kUseFirstInputIfItExists.
$Raises:
RuntimeError if
context.has_only_continuous_state() == false
.
- class TimeStep
- __init__(self: pydrake.planning.DirectTranscription.TimeStep, value: float) None
- property value
- class pydrake.planning.DistanceAndInterpolationProvider
This class represents the base interface for performing configuration distance and interpolation operations, used by CollisionChecker. See LinearDistanceAndInterpolationProvider for an implementation covering common “linear” distance and interpolation behavior.
Configuration distance and interpolation are necessary for a CollisionChecker to perform edge collision checks, and an essential part of many motion planning problems. The C-spaces for many planning problems combine joints with widely differing effects (e.g. for a given angular change, the shoulder joint of a robot arm results in much more significant motion than the same change on a finger joint) or units (e.g. a mobile robot with translation in meters and yaw in radians). As a result, it is often necessary to weight elements of the configuration differently when computing configuration distance.
Likewise, in more complex C-spaces, it may be necessary to perform more complex interpolation behavior (e.g. when planning for a mobile robot whose motion is modelled via Dubbins or Reeds-Shepp paths).
Configuration distance takes two configurations of the robot, from and to, both as Eigen::VectorXd, and returns (potentially weighted) C-space distance as a double. The returned distance will be strictly non-negative.
To be valid, distance must satisfy the following condition:
ComputeConfigurationDistance(q, q) ≡ 0
for values of q that are valid for the C-space in use.
Configuration interpolation takes two configurations of the robot, from and to, both as Eigen::VectorXd, plus a ratio in [0, 1] and returns the interpolated configuration.
To be valid, interpolation must satisfy the following conditions:
InterpolateBetweenConfigurations(from, to, 0) ≡ from
InterpolateBetweenConfigurations(from, to, 1) ≡ to
InterpolateBetweenConfigurations(q, q, ratio) ≡ q, for all ratio in [0, 1]
for values of q, from, and to that are valid for the C-space in use.
- __init__(*args, **kwargs)
- ComputeConfigurationDistance(self: pydrake.planning.DistanceAndInterpolationProvider, arg0: numpy.ndarray[numpy.float64[m, 1]], arg1: numpy.ndarray[numpy.float64[m, 1]]) float
Computes the configuration distance from the provided configuration
from
to the provided configurationto
. The returned distance will be strictly non-negative.- Precondition:
from.size() == to.size()
- InterpolateBetweenConfigurations(self: pydrake.planning.DistanceAndInterpolationProvider, arg0: numpy.ndarray[numpy.float64[m, 1]], arg1: numpy.ndarray[numpy.float64[m, 1]], arg2: float) numpy.ndarray[numpy.float64[m, 1]]
Returns the interpolated configuration between
from
andto
atratio
.- Precondition:
from.size() == to.size()
- Precondition:
ratio in [0, 1]
- class pydrake.planning.EdgeMeasure
The measure of the distance of the edge from q1 to q2 and the portion of that is collision free.
Distance is that produced by CollisionChecker::ComputeConfigurationDistance() for the entire edge between q1 and q2.
The portion of the edge between q1 and q2 that is collision free is encoded as the value α with the following semantics:
α = 1:
No collisions were detected. The full edge can be considered collision free. This is the only time completely_free() reports
True
. - 0 ≤ α < 1: A collision was detected between q1 and q2. α is the largest interpolation value such that an edge from q1 to qα can be considered collision free (where qα = interpolate(q1, q2, α)). partially_free() reportsTrue
. - α is undefined: q1 was found to be in collision. That means there exists no α for which the edge (q1, qα) can be collision free.Note
The length of the collision-free edge can be computed via distance * α. To simplify comparisons between a number of edges, some of which may not have a defined α, the function alpha_or(default_value) is provided. This is equivalent to
edge.partially_free() ? edge.alpha() : default_value
.Note
For α to be meaningful, the caller is obliged to make sure that they use the same interpolating function as the CollisionChecker did when generating the measure. Calling CollisionChecker::InterpolateBetweenConfigurations() on the same checker instance would satisfy that requirement.
- __init__(*args, **kwargs)
Overloaded function.
__init__(self: pydrake.planning.EdgeMeasure, distance: float, alpha: float) -> None
- Precondition:
0 ≤ distance
- Precondition:
0 ≤ alpha ≤ 1
to indicate definedalpha
, negative otherwise.
__init__(self: pydrake.planning.EdgeMeasure, other: pydrake.planning.EdgeMeasure) -> None
- alpha(self: pydrake.planning.EdgeMeasure) float
Returns the value of alpha, if defined.
Note: Due to the sampling nature of the edge check, the edge (q1, qα) may not actually be collision free (due to a missed collision). There’s a further subtlety. Subsequently calling CheckEdgeCollisionFree(q1, qα) may return
False
. This apparent contradiction is due to the fact that the samples on the edge (q1, qα) will not necessarily be the same as the samples originally tested on the edge (q1, q2). It is possible for those new samples to detect a previously missed collision. This is not a bug, merely a property of sampling-based testing.- Precondition:
partially_free() returns
True
.
- alpha_or(self: pydrake.planning.EdgeMeasure, default_value: float) float
Returns the value of alpha, if defined, or the provided default value.
- completely_free(self: pydrake.planning.EdgeMeasure) bool
Reports
True
if all samples were collision free.
- distance(self: pydrake.planning.EdgeMeasure) float
Returns the edge distance.
- partially_free(self: pydrake.planning.EdgeMeasure) bool
Reports
True
if there’s any portion of the edge (starting from q1) that is collision free. By implication, if completely_free() reportsTrue
, so will this.
- class pydrake.planning.GcsTrajectoryOptimization
GcsTrajectoryOptimization implements a simplified motion planning optimization problem introduced in the paper “Motion Planning around Obstacles with Convex Optimization” by Tobia Marcucci, Mark Petersen, David von Wrangel, Russ Tedrake.
Warning
This feature is considered to be experimental and may change or be removed at any time, without any deprecation notice ahead of time.
Instead of using the full time-scaling curve, this problem uses a single time-scaling variable for each region. This formulation yields continuous trajectories, which are not differentiable at the transition times between the regions since non-convex continuity constraints are not supported yet. However, it supports continuity on the path r(s) for arbitrary degree. The path r(s) can be reconstructed from the gcs solution q(t) with
NormalizeSegmentTimes()
and post-processed with e.g. Toppra to enforce acceleration bounds.The ith piece of the composite trajectory is defined as q(t) = r((t - tᵢ) / hᵢ). r : [0, 1] → ℝⁿ is a the path, parametrized as a Bézier curve with order n. tᵢ and hᵢ are the initial time and duration of the ith sub-trajectory.
This class supports the notion of a Subgraph of regions. This has proven useful to facilitate multi-modal motion planning such as: Subgraph A: find a collision-free trajectory for the robot to a grasping posture, Subgraph B: find a collision-free trajectory for the robot with the object in its hand to a placing posture, etc.
**** Continuous Revolute Joints
This class also supports robots with continuous revolute joints (revolute joints that don’t have any joint limits) and mobile bases. Adding or subtracting 2π to such a joint’s angle leaves it unchanged; this logic is implemented behind the scenes. To use it, one should specify the joint indices that don’t have limits, and ensure all sets satisfy the “convexity radius” property – their width along a dimension corresponding to a continuous revolute joint must be less than π. This can be enforced when constructing the convex sets, or after the fact with
geometry::optimization::PartitionConvexSet
. TheGcsTrajectoryOptimization
methodsAddRegions
andAddEdges
will handle all of the intersection checks behind the scenes, including applying the appropriate logic to connect sets that “wrap around” 2π.- __init__(self: pydrake.planning.GcsTrajectoryOptimization, num_positions: int, continuous_revolute_joints: list[int] = []) None
Constructs the motion planning problem.
- Parameter
num_positions
: is the dimension of the configuration space.
- Parameter
continuous_revolute_joints
: is a list of indices corresponding to continuous revolute joints, i.e., revolute joints which don’t have any joint limits, and hence “wrap around” at 2π. Each entry in continuous_revolute_joints must be non-negative, less than num_positions, and unique. This feature is currently only supported within a single subgraph: continuous revolute joints won’t be taken into account when constructing edges between subgraphs or checking if sets intersect through a subspace.
- Parameter
- AddContinuityConstraints(self: pydrake.planning.GcsTrajectoryOptimization, continuity_order: int) None
Enforces derivative continuity constraints on the entire graph.
This adds a nonlinear constraint to the restriction and MIP GraphOfConvexSets::Transcription, while adding a convex surrogate to the relaxation. For more details, see nonconvex_graph_of_convex_sets “Guiding Non-convex Optimization with the GraphOfConvexSets”.
The continuity is enforced on the control points of q(t), which appear as nonlinear constraints.
Click to expand C++ code...
(dᴺrᵤ(s=1) / dsᴺ) / hᵤᴺ == (dᴺrᵥ(s=0) / dsᴺ) / hᵥᴺ
The convex surrogate is simply the path continuity, where hᵤᴺ and hᵥᴺ are replaced by the characteristic times of the respective sets:
Click to expand C++ code...
(dᴺrᵤ(s=1) / dsᴺ) / hᵤ₀ᴺ == (dᴺrᵥ(s=0) / dsᴺ) / hᵥ₀ᴺ
. For now, these are set to one, but future work may involve scaling them by the size of the sets.
- Parameter
continuity_order
: is the order of the continuity constraint.
- Raises
RuntimeError if the continuity order is less than one since path –
continuity is enforced by default. –
- Parameter
- AddEdges(self: pydrake.planning.GcsTrajectoryOptimization, from_subgraph: pydrake.planning.GcsTrajectoryOptimization.Subgraph, to_subgraph: pydrake.planning.GcsTrajectoryOptimization.Subgraph, subspace: pydrake.geometry.optimization.ConvexSet = None, edges_between_regions: Optional[list[tuple[int, int]]] = None, edge_offsets: Optional[list[numpy.ndarray[numpy.float64[m, 1]]]] = None) pydrake.planning.GcsTrajectoryOptimization.EdgesBetweenSubgraphs
Connects two subgraphs with directed edges.
- Parameter
from_subgraph
: is the subgraph to connect from. Must have been created from a call to AddRegions() on this object, not some other optimization program.
- Parameter
to_subgraph
: is the subgraph to connect to. Must have been created from a call to AddRegions() on this object, not some other optimization program.
- Parameter
subspace
: is the subspace that the connecting control points must be in. Subspace is optional. Only edges that connect through the subspace will be added, and the subspace is added as a constraint on the connecting control points. Subspaces of type point or HPolyhedron are supported since other sets require constraints that are not yet supported by the GraphOfConvexSets::Edge constraint, e.g., set containment of a Hyperellipsoid is formulated via LorentzCone constraints. Workaround: Create a subgraph of zero order with the subspace as the region and connect it between the two subgraphs. This works because GraphOfConvexSet::Vertex supports arbitrary instances of ConvexSets.
- Parameter
edges_between_regions
: can be used to manually specify which edges should be added, avoiding the intersection checks. It should be a list of tuples
(i,j)
, where an edge will be added from thei`th index region in `from_subgraph
to thej`th index region in `to_subgraph
.- Parameter
edge_offsets
: is an optional list of vectors. If defined, the list must contain the same number of entries as
edges_between_regions
, and the order must match. In other words, if defined, there must be one edge offset for each specified edge, and they must be at the same index. For each pair of sets listed inedges_between_regions
, the first set is translated (in configuration space) by the corresponding vector in edge_offsets before computing the constraints associated to that edge. This is used to add edges between sets that “wrap around” 2π along some dimension, due to, e.g., a continuous revolute joint. This edge offset corresponds to the translation component of the affine map τ_uv in equation (11) of “Non-Euclidean Motion Planning with Graphs of Geodesically-Convex Sets”, and per the discussion in Subsection VI A, τ_uv has no rotation component. If edge_offsets is nullptr, it will instead be computed automatically.
- Raises
RuntimeError if edge_offsets is provided, but –
edge_offsets.size() != edges_between_regions.size()` –
- Parameter
- AddNonlinearDerivativeBounds(self: pydrake.planning.GcsTrajectoryOptimization, lb: numpy.ndarray[numpy.float64[m, 1]], ub: numpy.ndarray[numpy.float64[m, 1]], derivative_order: int) None
Adds a nonlinear derivative constraints to the entire graph
lb
≤ dᴺq(t) / dtᴺ ≤ub
.This adds a nonlinear constraint to the restriction and MIP GraphOfConvexSets::Transcription, while adding a convex surrogate to the relaxation. For more details, see nonconvex_graph_of_convex_sets “Guiding Non-convex Optimization with the GraphOfConvexSets”.
The nonlinear constraint involves the derivative dᴺq(t) / dtᴺ which is decomposed as dᴺr(s) / dsᴺ / hᴺ. The convex surrogate replaces the nonlinear component hᴺ with h₀ᴺ⁻¹h, where h₀ is the characteristic time of the set. For now, h₀ is set to 1.0 for all sets.
- Parameter
lb
: is the lower bound of the derivative.
- Parameter
ub
: is the upper bound of the derivative.
- Parameter
derivative_order
: is the order of the derivative to be constrained.
- Raises
RuntimeError if lb or ub are not of size num_positions() –
RuntimeError if the derivative order <= 1, since the linear –
velocity bounds are preferred. –
- Parameter
- AddPathContinuityConstraints(self: pydrake.planning.GcsTrajectoryOptimization, continuity_order: int) None
Enforces derivative continuity constraints on the entire graph.
Note that the constraints are on the control points of the derivatives of r(s) and not q(t). This may result in discontinuities of the trajectory return by
SolvePath()
since the r(s) will get rescaled by the duration h to yield q(t).NormalizeSegmentTimes()
will return r(s) with valid continuity.- Parameter
continuity_order
: is the order of the continuity constraint.
- Raises
RuntimeError if the continuity order is less than one since path –
continuity is enforced by default. –
- Parameter
- AddPathEnergyCost(*args, **kwargs)
Overloaded function.
AddPathEnergyCost(self: pydrake.planning.GcsTrajectoryOptimization, weight_matrix: numpy.ndarray[numpy.float64[m, n]]) -> None
Similar to AddPathLengthCost in usage, but minimizes ∑ |weight_matrix * (rᵢ₊₁ − rᵢ)|₂². In comparison to AddPathLength cost, this cost encourages control points to be evenly spaced but may result in greater number of regions and larger path length on the solution. It is recommended to use this cost only with SolveConvexRestriction when it becomes a quadratic cost for which some solvers show a better performance.
This cost will be added to the entire graph. Since the path length is only defined for Bézier curves that have two or more control points, this cost will only added to all subgraphs with order greater than zero. Note that this cost will be applied even to subgraphs added in the future.
- Parameter
weight_matrix
: is the relative weight of each component for the cost. The diagonal of the matrix is the weight for each dimension. The off-diagonal elements are the weight for the cross terms, which can be used to penalize diagonal movement.
- Precondition:
weight_matrix must be of size num_positions() x num_positions().
AddPathEnergyCost(self: pydrake.planning.GcsTrajectoryOptimization, weight: float = 1.0) -> None
Similar to AddPathLengthCost in usage, but minimizes ∑ |(rᵢ₊₁ − rᵢ)|₂² with weight being applied uniformly to all dimensions. In comparison to AddPathLength cost, this cost encourages control points to be evenly spaced but may result in greater number of regions and larger path length on the solution. It is recommended to use this cost only with SolveConvexRestriction when it becomes a quadratic cost for which some solvers show a better performance.
This cost will be added to the entire graph. Since the path length is only defined for Bézier curves that have two or more control points, this cost will only added to all subgraphs with order greater than zero. Note that this cost will be applied even to subgraphs added in the future.
- Parameter
weight
: is the relative weight of the cost.
- AddPathLengthCost(*args, **kwargs)
Overloaded function.
AddPathLengthCost(self: pydrake.planning.GcsTrajectoryOptimization, weight_matrix: numpy.ndarray[numpy.float64[m, n]]) -> None
Adds multiple L2Norm Costs on the upper bound of the path length. Since we cannot directly compute the path length of a Bézier curve, we minimize the upper bound of the path integral by minimizing the sum of (weighted) distances between control points: ∑ |weight_matrix * (rᵢ₊₁ − rᵢ)|₂.
This cost will be added to the entire graph. Since the path length is only defined for Bézier curves that have two or more control points, this cost will only added to all subgraphs with order greater than zero. Note that this cost will be applied even to subgraphs added in the future.
- Parameter
weight_matrix
: is the relative weight of each component for the cost. The diagonal of the matrix is the weight for each dimension. The off-diagonal elements are the weight for the cross terms, which can be used to penalize diagonal movement.
- Precondition:
weight_matrix must be of size num_positions() x num_positions().
AddPathLengthCost(self: pydrake.planning.GcsTrajectoryOptimization, weight: float = 1.0) -> None
Adds multiple L2Norm Costs on the upper bound of the path length. Since we cannot directly compute the path length of a Bézier curve, we minimize the upper bound of the path integral by minimizing the sum of distances between control points. For Bézier curves, this is equivalent to the sum of the L2Norm of the derivative control points of the curve divided by the order.
This cost will be added to the entire graph. Since the path length is only defined for Bézier curves that have two or more control points, this cost will only added to all subgraphs with order greater than zero. Note that this cost will be applied even to subgraphs added in the future.
- Parameter
weight
: is the relative weight of the cost.
- AddRegions(*args, **kwargs)
Overloaded function.
AddRegions(self: pydrake.planning.GcsTrajectoryOptimization, regions: list[pydrake.geometry.optimization.ConvexSet], edges_between_regions: list[tuple[int, int]], order: int, h_min: float = 1e-06, h_max: float = 20, name: str = ‘’, edge_offsets: Optional[list[numpy.ndarray[numpy.float64[m, 1]]]] = None) -> pydrake.planning.GcsTrajectoryOptimization.Subgraph
Creates a Subgraph with the given regions and indices.
- Parameter
regions
: represent the valid set a control point can be in. We retain a copy of the regions since other functions may access them. If any of the positions represent revolute joints without limits, each region has a maximum width of strictly less than π along dimensions corresponding to those joints.
- Parameter
edges_between_regions
: is a list of pairs of indices into the regions vector. For each pair representing an edge between two regions, an edge is added within the subgraph. Note that the edges are directed so (i,j) will only add an edge from region i to region j.
- Parameter
order
: is the order of the Bézier curve.
- Parameter
h_max
: is the maximum duration to spend in a region (seconds). Some solvers struggle numerically with large values.
- Parameter
h_min
: is the minimum duration to spend in a region (seconds) if that region is visited on the optimal path. Some cost and constraints are only convex for h > 0. For example the perspective quadratic cost of the path energy ||ṙ(s)||² / h becomes non-convex for h = 0. Otherwise h_min can be set to 0.
- Parameter
name
: is the name of the subgraph. If the passed name is an empty string, a default name will be provided.
- Parameter
edge_offsets
: is an optional list of vectors. If defined, the list must contain the same number of entries as
edges_between_regions
. For each pair of sets listed inedges_between_regions
, the first set is translated (in configuration space) by the corresponding vector in edge_offsets before computing the constraints associated to that edge. This is used to add edges between sets that “wrap around” 2π along some dimension, due to, e.g., a continuous revolute joint. This edge offset corresponds to the translation component of the affine map τ_uv in equation (11) of “Non-Euclidean Motion Planning with Graphs of Geodesically-Convex Sets”, and per the discussion in Subsection VI A, τ_uv has no rotation component. If edge_offsets is nullptr, it will instead be computed automatically.
- Raises
RuntimeError if any index referenced in edges_between_regions –
is outside the range [0, ssize(regions)) –
AddRegions(self: pydrake.planning.GcsTrajectoryOptimization, regions: list[pydrake.geometry.optimization.ConvexSet], order: int, h_min: float = 1e-06, h_max: float = 20, name: str = ‘’) -> pydrake.planning.GcsTrajectoryOptimization.Subgraph
Creates a Subgraph with the given regions. This function will compute the edges between the regions based on the set intersections.
- Parameter
regions
: represent the valid set a control point can be in. We retain a copy of the regions since other functions may access them. If any of the positions represent continuous revolute joints, each region must have a maximum width of strictly less than π along dimensions corresponding to those joints.
- Parameter
order
: is the order of the Bézier curve.
- Parameter
h_min
: is the minimum duration to spend in a region (seconds) if that region is visited on the optimal path. Some cost and constraints are only convex for h > 0. For example the perspective quadratic cost of the path energy ||ṙ(s)||² / h becomes non-convex for h = 0. Otherwise h_min can be set to 0.
- Parameter
h_max
: is the maximum duration to spend in a region (seconds). Some solvers struggle numerically with large values.
- Parameter
name
: is the name of the subgraph. A default name will be provided.
- Raises
RuntimeError if any of the regions has a width of π or greater –
along dimensions corresponding to continuous revolute joints. –
- AddTimeCost(self: pydrake.planning.GcsTrajectoryOptimization, weight: float = 1.0) None
Adds a minimum time cost to all regions in the whole graph. The cost is the sum of the time scaling variables.
This cost will be added to the entire graph. Note that this cost will be applied even to subgraphs added in the future.
- Parameter
weight
: is the relative weight of the cost.
- Parameter
- AddVelocityBounds(self: pydrake.planning.GcsTrajectoryOptimization, lb: numpy.ndarray[numpy.float64[m, 1]], ub: numpy.ndarray[numpy.float64[m, 1]]) None
Adds a linear velocity constraint to the entire graph
lb
≤ q̇(t) ≤ub
.- Parameter
lb
: is the lower bound of the velocity.
- Parameter
ub
: is the upper bound of the velocity.
This constraint will be added to the entire graph. Since the velocity requires forming the derivative of the Bézier curve, this constraint will only added to all subgraphs with order greater than zero. Note that this constraint will be applied even to subgraphs added in the future.
- Raises
RuntimeError if lb or ub are not of size num_positions() –
- Parameter
- continuous_revolute_joints(self: pydrake.planning.GcsTrajectoryOptimization) list[int]
Returns a list of indices corresponding to continuous revolute joints.
- class EdgesBetweenSubgraphs
EdgesBetweenSubgraphs are defined as the connecting edges between two given subgraphs. These edges are a subset of the many other edges in the larger graph. From an API standpoint, EdgesBetweenSubgraphs enable transitions between Subgraphs, which can enable transitions between modes. Further, it allows different constraints to be added in the transition between subgraphs. Note that the EdgesBetweenSubgraphs can’t be separated from the actual edges in the GraphOfConvexSets framework, thus mixing it with other instances of GCSTrajetoryOptimization is not supported.
- __init__(*args, **kwargs)
- AddContinuityConstraints(self: pydrake.planning.GcsTrajectoryOptimization.EdgesBetweenSubgraphs, continuity_order: int) None
Enforces derivative continuity constraints on the edges between the subgraphs.
This adds a nonlinear constraint to the restriction and MIP GraphOfConvexSets::Transcription, while adding a convex surrogate to the relaxation. For more details, see nonconvex_graph_of_convex_sets “Guiding Non-convex Optimization with the GraphOfConvexSets”.
The continuity is enforced on the control points of q(t), which appear as nonlinear constraints.
Click to expand C++ code...
(dᴺrᵤ(s=1) / dsᴺ) / hᵤᴺ == (dᴺrᵥ(s=0) / dsᴺ) / hᵥᴺ
The convex surrogate is simply the path continuity, where hᵤᴺ and hᵥᴺ are replaced by the characteristic times of the respective sets:
Click to expand C++ code...
(dᴺrᵤ(s=1) / dsᴺ) / hᵤ₀ᴺ == (dᴺrᵥ(s=0) / dsᴺ) / hᵥ₀ᴺ
. For now, these are set to one, but future work may involve scaling them by the size of the sets.
- Parameter
continuity_order
: is the order of the continuity constraint.
- Raises
RuntimeError if the continuity order is not equal or less than the –
order of both subgraphs. –
RuntimeError if the continuity order is less than one since path –
continuity is enforced by default. –
- Parameter
- AddNonlinearDerivativeBounds(self: pydrake.planning.GcsTrajectoryOptimization.EdgesBetweenSubgraphs, lb: numpy.ndarray[numpy.float64[m, 1]], ub: numpy.ndarray[numpy.float64[m, 1]], derivative_order: int) None
Adds a nonlinear derivative constraints to the control point connecting the subgraphs
lb
≤ dᴺq(t) / dtᴺ ≤ub
.This adds a nonlinear constraint to the restriction and MIP GraphOfConvexSets::Transcription, while adding a convex surrogate to the relaxation. For more details, see nonconvex_graph_of_convex_sets “Guiding Non-convex Optimization with the GraphOfConvexSets”.
The nonlinear constraint involves the derivative dᴺq(t) / dtᴺ which is decomposed as dᴺr(s) / dsᴺ / hᴺ. The convex surrogate replaces the nonlinear component hᴺ with h₀ᴺ⁻¹h, where h₀ is the characteristic time of the set. For now, h₀ is set to 1.0 for all sets.
- Parameter
lb
: is the lower bound of the derivative.
- Parameter
ub
: is the upper bound of the derivative.
- Parameter
derivative_order
: is the order of the derivative to be constrained.
- Raises
RuntimeError if both subgraphs order is less than the desired –
derivative order. –
RuntimeError if the derivative order <= 1, since the linear –
velocity bounds are preferred. –
RuntimeError if lb or ub are not of size num_positions() –
- Parameter
- AddPathContinuityConstraints(self: pydrake.planning.GcsTrajectoryOptimization.EdgesBetweenSubgraphs, continuity_order: int) None
Enforces derivative continuity constraints on the edges between the subgraphs.
Note that the constraints are on the control points of the derivatives of r(s) and not q(t). This may result in discontinuities of the trajectory return by
SolvePath()
since the r(s) will get rescaled by the duration h to yield q(t).NormalizeSegmentTimes()
will return r(s) with valid continuity.- Parameter
continuity_order
: is the order of the continuity constraint.
- Raises
RuntimeError if the continuity order is not equal or less than the –
order of both subgraphs. –
RuntimeError if the continuity order is less than one since path –
continuity is enforced by default. –
- Parameter
- AddVelocityBounds(self: pydrake.planning.GcsTrajectoryOptimization.EdgesBetweenSubgraphs, lb: numpy.ndarray[numpy.float64[m, 1]], ub: numpy.ndarray[numpy.float64[m, 1]]) None
Adds a linear velocity constraint to the control point connecting the subgraphs
lb
≤ q̇(t) ≤ub
.- Parameter
lb
: is the lower bound of the velocity.
- Parameter
ub
: is the upper bound of the velocity.
- Raises
RuntimeError if both subgraphs order is zero, since the velocity –
is defined as the derivative of the Bézier curve. At least one of –
the subgraphs must have an order of at least 1. –
RuntimeError if lb or ub are not of size num_positions() –
- Parameter
- AddZeroDerivativeConstraints(self: pydrake.planning.GcsTrajectoryOptimization.EdgesBetweenSubgraphs, derivative_order: int) None
Enforces zero derivatives on the control point connecting the subgraphs.
For velocity, acceleration, jerk, etc. enforcing zero-derivative on the trajectory q(t) is equivalent to enforcing zero-derivative on the trajectory r(s). Hence this constraint is convex.
- Parameter
derivative_order
: is the order of the derivative to be constrained.
- Raises
RuntimeError if the derivative order < 1. –
RuntimeError if both subgraphs order is less than the desired –
derivative order. –
- Parameter
- Edges(self: pydrake.planning.GcsTrajectoryOptimization.EdgesBetweenSubgraphs) list[pydrake.geometry.optimization.GraphOfConvexSets.Edge]
Returns constant reference to a vector of mutable pointers to the edges.
- GetEdgesBetweenSubgraphs(self: pydrake.planning.GcsTrajectoryOptimization) list[pydrake.planning.GcsTrajectoryOptimization.EdgesBetweenSubgraphs]
Returns a vector of all edges between subgraphs.
- GetGraphvizString(*args, **kwargs)
Overloaded function.
GetGraphvizString(self: pydrake.planning.GcsTrajectoryOptimization, 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)) -> 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.- Parameter
show_slacks
: determines whether the values of the intermediate (slack) variables are also displayed in the graph.
- Parameter
precision
: sets the floating point precision (how many digits are generated) of the annotations.
- Parameter
scientific
: sets the floating point formatting to scientific (if true) or fixed (if false).
GetGraphvizString(self: pydrake.planning.GcsTrajectoryOptimization, 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) -> 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.- Parameter
show_slacks
: determines whether the values of the intermediate (slack) variables are also displayed in the graph.
- Parameter
precision
: sets the floating point precision (how many digits are generated) of the annotations.
- Parameter
scientific
: sets the floating point formatting to scientific (if true) or fixed (if false).
- GetSubgraphs(self: pydrake.planning.GcsTrajectoryOptimization) list[pydrake.planning.GcsTrajectoryOptimization.Subgraph]
Returns a vector of all subgraphs.
- graph_of_convex_sets(self: pydrake.planning.GcsTrajectoryOptimization) pydrake.geometry.optimization.GraphOfConvexSets
Getter for the underlying GraphOfConvexSets. This is intended primarily for inspecting the resulting programs.
- static NormalizeSegmentTimes(trajectory: pydrake.trajectories.CompositeTrajectory) pydrake.trajectories.CompositeTrajectory
Normalizes each trajectory segment to one second in duration. Reconstructs the path r(s) from the solution trajectory q(t) of
SolvePath()
s.t. each segment of the resulting trajectory will be one second long. The start time will match the original start time.- Parameter
trajectory
: The solution trajectory returned by
SolvePath()
.
- Raises
RuntimeError if not all trajectory segments of the –
CompositeTrajectory are of type BezierCurve<double> –
- Parameter
- num_positions(self: pydrake.planning.GcsTrajectoryOptimization) int
Returns the number of position variables.
- RemoveSubgraph(self: pydrake.planning.GcsTrajectoryOptimization, subgraph: pydrake.planning.GcsTrajectoryOptimization.Subgraph) None
Remove a subgraph and all associated edges found in the subgraph and to and from other subgraphs.
- Precondition:
The subgraph must have been created from a call to AddRegions() on this object.
- SolveConvexRestriction(self: pydrake.planning.GcsTrajectoryOptimization, active_vertices: list[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, )) tuple[pydrake.trajectories.CompositeTrajectory, pydrake.solvers.MathematicalProgramResult]
Solves a trajectory optimization problem through specific vertices.
This method allows for targeted optimization by considering only selected active vertices, reducing the problem’s complexity. See geometry::optimization::GraphOfConvexSets::SolveConvexRestriction(). This API prefers a sequence of vertices over edges, as a user may know which regions the solution should pass through. GcsTrajectoryOptimization::AddRegions() automatically manages edge creation and intersection checks, which makes passing a sequence of edges less convenient.
- Parameter
active_vertices
: A sequence of ordered vertices of subgraphs to be included in the problem.
- Parameter
options
: include all settings for solving the shortest path problem.
- Precondition:
There must be at least two vertices in active_vertices.
- Raises
RuntimeError if the vertices are not connected. –
RuntimeError if two vertices are connected by multiple edges. This –
may happen if one connects two graphs through multiple subspaces, –
which is currently not supported with this method. –
RuntimeError if the program cannot be written as a convex –
optimization consumable by one of the standard solvers. –
- Parameter
- SolvePath(self: pydrake.planning.GcsTrajectoryOptimization, source: pydrake.planning.GcsTrajectoryOptimization.Subgraph, target: pydrake.planning.GcsTrajectoryOptimization.Subgraph, 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, )) tuple[pydrake.trajectories.CompositeTrajectory, pydrake.solvers.MathematicalProgramResult]
Formulates and solves the mixed-integer convex formulation of the shortest path problem on the whole graph.
See also
geometry::optimization::GraphOfConvexSets::SolveShortestPath()
for further details.- Parameter
source
: specifies the source subgraph. Must have been created from a call to AddRegions() on this object, not some other optimization program. If the source is a subgraph with more than one region, an empty set will be added and optimizer will choose the best region to start in. To start in a particular point, consider adding a subgraph of order zero with a single region of type Point.
- Parameter
target
: specifies the target subgraph. Must have been created from a call to AddRegions() on this object, not some other optimization program. If the target is a subgraph with more than one region, an empty set will be added and optimizer will choose the best region to end in. To end in a particular point, consider adding a subgraph of order zero with a single region of type Point.
- Parameter
options
: include all settings for solving the shortest path problem. The following default options will be used if they are not provided in
options
: - options.convex_relaxation = true, -options.max_rounded_paths = 5
, - options.preprocessing = true.
See also
geometry::optimization::GraphOfConvexSetsOptions
for further details.- Parameter
- class Subgraph
A Subgraph is a subset of the larger graph. It is defined by a set of regions and edges between them based on intersection. From an API standpoint, a Subgraph is useful to define a multi-modal motion planning problem. Further, it allows different constraints and objects to be added to different subgraphs. Note that the the GraphOfConvexSets does not differentiate between subgraphs and can’t be mixed with other instances of GcsTrajectoryOptimization.
- __init__(*args, **kwargs)
- AddContinuityConstraints(self: pydrake.planning.GcsTrajectoryOptimization.Subgraph, continuity_order: int) None
Enforces derivative continuity constraints on the subgraph.
This adds a nonlinear constraint to the restriction and MIP GraphOfConvexSets::Transcription, while adding a convex surrogate to the relaxation. For more details, see nonconvex_graph_of_convex_sets “Guiding Non-convex Optimization with the GraphOfConvexSets”.
The continuity is enforced on the control points of q(t), which appear as nonlinear constraints.
Click to expand C++ code...
(dᴺrᵤ(s=1) / dsᴺ) / hᵤᴺ == (dᴺrᵥ(s=0) / dsᴺ) / hᵥᴺ
The convex surrogate is simply the path continuity, where hᵤᴺ and hᵥᴺ are replaced by the characteristic times of the respective sets:
Click to expand C++ code...
(dᴺrᵤ(s=1) / dsᴺ) / hᵤ₀ᴺ == (dᴺrᵥ(s=0) / dsᴺ) / hᵥ₀ᴺ
. For now, these are set to one, but future work may involve scaling them by the size of the sets.
- Parameter
continuity_order
: is the order of the continuity constraint.
- Raises
RuntimeError if the continuity order is not equal or less than the –
order the subgraphs. –
RuntimeError if the continuity order is less than one since path –
continuity is enforced by default. –
- Parameter
- AddNonlinearDerivativeBounds(self: pydrake.planning.GcsTrajectoryOptimization.Subgraph, lb: numpy.ndarray[numpy.float64[m, 1]], ub: numpy.ndarray[numpy.float64[m, 1]], derivative_order: int) None
Adds a nonlinear derivative constraints to the subgraph
lb
≤ dᴺq(t) / dtᴺ ≤ub
.This adds a nonlinear constraint to the restriction and MIP GraphOfConvexSets::Transcription, while adding a convex surrogate to the relaxation. For more details, see nonconvex_graph_of_convex_sets “Guiding Non-convex Optimization with the GraphOfConvexSets”.
The nonlinear constraint involves the derivative dᴺq(t) / dtᴺ which is decomposed as dᴺr(s) / dsᴺ / hᴺ. The convex surrogate replaces the nonlinear component hᴺ with h₀ᴺ⁻¹h, where h₀ is the characteristic time of the set. For now, h₀ is set to 1.0 for all sets.
- Parameter
lb
: is the lower bound of the derivative.
- Parameter
ub
: is the upper bound of the derivative.
- Parameter
derivative_order
: is the order of the derivative to be constrained.
- Raises
RuntimeError if subgraph order is less than the derivative order. –
RuntimeError if the derivative order <= 1, since the linear –
velocity bounds are preferred. –
RuntimeError if lb or ub are not of size num_positions() –
- Parameter
- AddPathContinuityConstraints(self: pydrake.planning.GcsTrajectoryOptimization.Subgraph, continuity_order: int) None
Enforces derivative continuity constraints on the subgraph.
Note that the constraints are on the control points of the derivatives of r(s) and not q(t). This may result in discontinuities of the trajectory return by
SolvePath()
since the r(s) will get rescaled by the duration h to yield q(t).NormalizeSegmentTimes()
will return r(s) with valid continuity.- Parameter
continuity_order
: is the order of the continuity constraint.
- Raises
RuntimeError if the continuity order is not equal or less than the –
order the subgraphs. –
RuntimeError if the continuity order is less than one since path –
continuity is enforced by default. –
- Parameter
- AddPathEnergyCost(*args, **kwargs)
Overloaded function.
AddPathEnergyCost(self: pydrake.planning.GcsTrajectoryOptimization.Subgraph, weight_matrix: numpy.ndarray[numpy.float64[m, n]]) -> None
Similar to AddPathLengthCost in usage, but minimizes ∑ |weight_matrix * (rᵢ₊₁ − rᵢ)|₂². In comparison to AddPathLength cost, this cost encourages control points to be evenly spaced but may result in greater number of regions and larger path length on the solution. It is recommended to use this cost only with SolveConvexRestriction when it becomes a quadratic cost for which some solvers show a better performance.
- Parameter
weight_matrix
: is the relative weight of each component for the cost. The diagonal of the matrix is the weight for each dimension. The off-diagonal elements are the weight for the cross terms, which can be used to penalize diagonal movement.
- Precondition:
weight_matrix must be of size num_positions() x num_positions().
AddPathEnergyCost(self: pydrake.planning.GcsTrajectoryOptimization.Subgraph, weight: float = 1.0) -> None
Similar to AddPathLengthCost in usage, but minimizes ∑ |(rᵢ₊₁ − rᵢ)|₂² with weight being applied uniformly to all dimensions. In comparison to AddPathLength cost, this cost encourages control points to be evenly spaced but may result in greater number of regions and larger path length on the solution. It is recommended to use this cost only with SolveConvexRestriction when it becomes a quadratic cost for which some solvers show a better performance.
- Parameter
weight
: is the relative weight of the cost.
- AddPathLengthCost(*args, **kwargs)
Overloaded function.
AddPathLengthCost(self: pydrake.planning.GcsTrajectoryOptimization.Subgraph, weight_matrix: numpy.ndarray[numpy.float64[m, n]]) -> None
Adds multiple L2Norm Costs on the upper bound of the path length. Since we cannot directly compute the path length of a Bézier curve, we minimize the upper bound of the path integral by minimizing the sum of distances between control points. For Bézier curves, this is equivalent to the sum of the L2Norm of the derivative control points of the curve divided by the order.
- Parameter
weight_matrix
: is the relative weight of each component for the cost. The diagonal of the matrix is the weight for each dimension. The off-diagonal elements are the weight for the cross terms, which can be used to penalize diagonal movement.
- Precondition:
weight_matrix must be of size num_positions() x num_positions().
AddPathLengthCost(self: pydrake.planning.GcsTrajectoryOptimization.Subgraph, weight: float = 1.0) -> None
Adds multiple L2Norm Costs on the upper bound of the path length. We upper bound the trajectory length by the sum of the distances between control points. For Bézier curves, this is equivalent to the sum of the L2Norm of the derivative control points of the curve divided by the order.
- Parameter
weight
: is the relative weight of the cost.
- AddTimeCost(self: pydrake.planning.GcsTrajectoryOptimization.Subgraph, weight: float = 1.0) None
Adds a minimum time cost to all regions in the subgraph. The cost is the sum of the time scaling variables.
- Parameter
weight
: is the relative weight of the cost.
- Parameter
- AddVelocityBounds(self: pydrake.planning.GcsTrajectoryOptimization.Subgraph, lb: numpy.ndarray[numpy.float64[m, 1]], ub: numpy.ndarray[numpy.float64[m, 1]]) None
Adds a linear velocity constraint to the subgraph
lb
≤ q̇(t) ≤ub
.- Parameter
lb
: is the lower bound of the velocity.
- Parameter
ub
: is the upper bound of the velocity.
- Raises
RuntimeError if subgraph order is zero, since the velocity is –
defined as the derivative of the Bézier curve. –
RuntimeError if lb or ub are not of size num_positions() –
- Parameter
- Edges(self: pydrake.planning.GcsTrajectoryOptimization.Subgraph) list[pydrake.geometry.optimization.GraphOfConvexSets.Edge]
Returns constant reference to a vector of mutable pointers to the edges.
- name(self: pydrake.planning.GcsTrajectoryOptimization.Subgraph) str
Returns the name of the subgraph.
- order(self: pydrake.planning.GcsTrajectoryOptimization.Subgraph) int
Returns the order of the Bézier trajectory within the region.
- regions(self: pydrake.planning.GcsTrajectoryOptimization.Subgraph) object
Returns the regions associated with this subgraph before the CartesianProduct.
- size(self: pydrake.planning.GcsTrajectoryOptimization.Subgraph) int
Returns the number of vertices in the subgraph.
- Vertices(self: pydrake.planning.GcsTrajectoryOptimization.Subgraph) list[pydrake.geometry.optimization.GraphOfConvexSets.Vertex]
Returns constant reference to a vector of mutable pointers to the vertices stored in the subgraph. The order of the vertices is the same as the order the regions were added.
- static UnwrapToContinousTrajectory(gcs_trajectory: pydrake.trajectories.CompositeTrajectory, continuous_revolute_joints: list[int], starting_rounds: Optional[list[int]] = None, tol: float = 1e-08) pydrake.trajectories.CompositeTrajectory
Unwraps a trajectory with continuous revolute joints into a continuous trajectory in the Euclidean space. Trajectories produced by GcsTrajectoryOptimization for robotic systems with continuous revolute joints may include apparent discontinuities, where a multiple of 2π is instantaneously added to a joint value at the boundary between two adjacent segments of the trajectory. This function removes such discontinuities by adding or subtracting the appropriate multiple of 2π, “unwrapping” the trajectory into a continuous representation suitable for downstream tasks that do not take the joint wraparound into account.
- Parameter
gcs_trajectory
: The trajectory to unwrap.
- Parameter
continuous_revolute_joints
: The indices of the continuous revolute joints.
- Parameter
tol
: The numerical tolerance used to determine if two subsequent segments start and end at the same value modulo 2π for continuous revolute joints.
- Parameter
starting_rounds
: A vector of integers that sets the starting rounds for each continuous revolute joint. Given integer k for the starting_round of a joint, its initial position will be wrapped into [2πk , 2π(k+1)). If the starting rounds are not provided, the initial position of
trajectory
will be unchanged.
- Returns
an unwrapped (continous in Euclidean space) CompositeTrajectory.
- Raises
RuntimeError if –
starting_rounds.size()!=continuous_revolute_joints.size() –
RuntimeError if continuous_revolute_joints contain repeated –
indices and/or indices outside the range [0, –
gcs_trajectory.rows()) –
RuntimeError if the gcs_trajectory is not continuous on the –
manifold defined by the continuous_revolute_joints, i.e., the –
shift between two consecutive segments is not an integer multiple –
of 2π (within a tolerance of tol radians) –
RuntimeError if all the segments are not of type BezierCurve. –
Other types are not supported yet. Note that currently the output –
of GcsTrajectoryOptimization::SolvePath() is a CompositeTrajectory –
of BezierCurves. –
- Parameter
- pydrake.planning.GetContinuousRevoluteJointIndices(plant: pydrake.multibody.plant.MultibodyPlant) list[int]
Returns a list of indices in the plant’s generalized positions which correspond to a continuous revolute joint (a revolute joint with no joint limits). This includes the revolute component of PlanarJoint and RpyFloatingJoint.
- class pydrake.planning.IrisFromCliqueCoverOptions
- __init__(self: pydrake.planning.IrisFromCliqueCoverOptions) None
- property coverage_termination_threshold
The fraction of the domain that must be covered before we terminate the algorithm.
- property iris_options
The options used on internal calls to Iris. Currently, it is recommended to only run Iris for one iteration when building from a clique so as to avoid discarding the information gained from the clique.
Note that
IrisOptions
can optionally include a meshcat instance to provide debugging visualization. If this is providedIrisFromCliqueCover
will provide debug visualization in meshcat showing where in configuration space it is drawing from. However, if the parallelism option is set to allow more than 1 thread, then the debug visualizations of internal Iris calls will be disabled. This is due to a limitation of drawing to meshcat from outside the main thread.
- property iteration_limit
The maximum number of iterations of the algorithm.
- property minimum_clique_size
The minimum size of the cliques used to construct a region. If this is set lower than the ambient dimension of the space we are trying to cover, then this option will be overridden to be at least 1 + the ambient dimension.
- property num_points_per_coverage_check
The number of points to sample when testing coverage.
- property num_points_per_visibility_round
Number of points to sample when building visibilty cliques. If this option is less than twice the minimum clique size, it will be overridden to be at least twice the minimum clique size. If the algorithm ever fails to find a single clique in a visibility round, then the number of points in a visibility round will be doubled.
- property parallelism
The amount of parallelism to use. This algorithm makes heavy use of parallelism at many points and thus it is highly recommended to set this to the maximum tolerable parallelism.
- property point_in_set_tol
The tolerance used for checking whether a point is contained inside an HPolyhedron. See @ConvexSet::PointInSet.
- property rank_tol_for_minimum_volume_circumscribed_ellipsoid
The rank tolerance used for computing the MinimumVolumeCircumscribedEllipsoid of a clique. See @MinimumVolumeCircumscribedEllipsoid.
- pydrake.planning.IrisInConfigurationSpaceFromCliqueCover(checker: pydrake.planning.CollisionChecker, options: pydrake.planning.IrisFromCliqueCoverOptions, generator: pydrake.common.RandomGenerator, sets: list[pydrake.geometry.optimization.HPolyhedron], max_clique_solver: pydrake.planning.MaxCliqueSolverBase = None) list[pydrake.geometry.optimization.HPolyhedron]
Cover the configuration space in Iris regions using the Visibility Clique Cover Algorithm as described in
P. Werner, A. Amice, T. Marcucci, D. Rus, R. Tedrake “Approximating Robot Configuration Spaces with few Convex Sets using Clique Covers of Visibility Graphs” In 2024 IEEE Internation Conference on Robotics and Automation. https://arxiv.org/abs/2310.02875
- Parameter
checker
: The collision checker containing the plant and its associated scene_graph.
- Parameter
generator
: There are points in the algorithm requiring randomness. The generator controls this source of randomness.
- Parameter
sets
: [in/out] initial sets covering the space (potentially empty). The cover is written into this vector.
- Parameter
max_clique_solver
: The min clique cover problem is approximatley solved by repeatedly solving max clique on the uncovered graph and adding this largest clique to the cover. The max clique problem is solved by this solver. If parallelism is set to allow more than 1 thread, then the solver must be implemented in C++.
If nullptr is passed as the
max_clique_solver
, then max clique will be solved using an instance of MaxCliqueSolverViaGreedy, which is a fast heuristic. If higher quality cliques are desired, consider changing the solver to an instance of MaxCliqueSolverViaMip. Currently, the padding in the collision checker is not forwarded to the algorithm, and therefore the final regions do not necessarily respect this padding. Effectively, this means that the regions are generated as if the padding is set to 0. This behavior may be adjusted in the future at the resolution of #18830.Note that MaxCliqueSolverViaMip requires the availability of a Mixed-Integer Linear Programming solver (e.g. Gurobi and/or Mosek). We recommend enabling those solvers if possible because they produce higher quality cliques (https://drake.mit.edu/bazel.html#proprietary_solvers). The method will throw if
max_clique_solver
cannot solve the max clique problem.- Parameter
- class pydrake.planning.KinematicTrajectoryOptimization
Optimizes a trajectory, q(t) subject to costs and constraints on the trajectory and its derivatives. This is accomplished using a
path
, r(s), represented as a BsplineTrajectory on the interval s∈[0,1], and a separate duration, T, which maps [0,1] => [0,T].The q(t) trajectory is commonly associated with, for instance, the generalized positions of a MultibodyPlant by adding multibody costs and constraints; in this case take note that the velocities in this optimization are q̇(t), not v(t).
Use solvers::Solve to solve the problem. A typical use case could look like:
Click to expand C++ code...
KinematicTrajectoryOptimization trajopt(2, 10); // add costs and constraints trajopt.SetInitialGuess(...); auto result = solvers::Solve(trajopt.prog()); auto traj = trajopt.ReconstructTrajectory(result);
When possible this class attempts to formulate convex forms of the costs and constraints.
- __init__(*args, **kwargs)
Overloaded function.
__init__(self: pydrake.planning.KinematicTrajectoryOptimization, num_positions: int, num_control_points: int, spline_order: int = 4, duration: float = 1.0) -> None
Constructs an optimization problem for a position trajectory represented as a B-spline. The initial guess is the zero trajectory over the time interval [0, T].
- Parameter
num_positions
: The number of rows in the B-spline.
- Parameter
num_control_points
: The number of B-spline control points.
- Parameter
spline_order
: The order of the B-spline.
- Parameter
duration
: The duration (in seconds) of the initial guess.
__init__(self: pydrake.planning.KinematicTrajectoryOptimization, trajectory: pydrake.trajectories.BsplineTrajectory) -> None
Constructs an optimization problem for a trajectory represented by a B-spline with the same order and number of control points as
trajectory
. Additionally setstrajectory
as the initial guess for the optimization.
- AddAccelerationBounds(self: pydrake.planning.KinematicTrajectoryOptimization, lb: numpy.ndarray[numpy.float64[m, 1]], ub: numpy.ndarray[numpy.float64[m, 1]]) list[list[pydrake.solvers.Binding[Constraint]]]
Adds generic (nonlinear) constraints to enforce the upper and lower bounds to the acceleration trajectory, q̈(t). These constraints will be respected at all times. Note that this does NOT directly constrain r̈(s).
- Returns
A vector of bindings with the ith element is itself a vector of constraints (one per dof) adding a constraint to the ith control point of the acceleration trajectory.
- AddDurationConstraint(self: pydrake.planning.KinematicTrajectoryOptimization, lb: Optional[float], ub: Optional[float]) pydrake.solvers.Binding[BoundingBoxConstraint]
Adds bounding box constraints for upper and lower bounds on the duration of the trajectory.
- AddDurationCost(self: pydrake.planning.KinematicTrajectoryOptimization, weight: float = 1.0) pydrake.solvers.Binding[LinearCost]
Adds a linear cost on the duration of the trajectory.
- AddJerkBounds(self: pydrake.planning.KinematicTrajectoryOptimization, lb: numpy.ndarray[numpy.float64[m, 1]], ub: numpy.ndarray[numpy.float64[m, 1]]) list[list[pydrake.solvers.Binding[Constraint]]]
Adds generic (nonlinear) constraints to enforce the upper and lower bounds to the jerk trajectory, d³qdt³(t). These constraints will be respected at all times. Note that this does NOT directly constrain d³rds³(s).
- Returns
A vector of bindings with the ith element is itself a vector of constraints (one per dof) adding a constraint to the ith control point of the jerk trajectory.
- AddPathAccelerationConstraint(self: pydrake.planning.KinematicTrajectoryOptimization, lb: numpy.ndarray[numpy.float64[m, 1]], ub: numpy.ndarray[numpy.float64[m, 1]], s: float) pydrake.solvers.Binding[LinearConstraint]
Adds a linear constraint on the second derivative of the path,
lb
≤ r̈(s) ≤ub
. Note that this does NOT directly constrain q̈(t).- Precondition:
0 <=
s
<= 1.
- AddPathEnergyCost(self: pydrake.planning.KinematicTrajectoryOptimization, weight: float = 1.0) list[pydrake.solvers.Binding[Cost]]
Adds a convex quadratic cost on an upper bound on the energy of the path, ∫₀¹ |ṙ(s)|₂² ds, by summing the squared distance between the path control points. In the limit of infinitely many control points, minimizers for AddPathLengthCost and AddPathEnergyCost will follow the same path, but potentially with different timing. They may have different values if additional costs and constraints are imposed. This cost yields simpler gradients than AddPathLengthCost, and biases the control points towards being evenly spaced.
- Returns
A vector of bindings with the ith element adding a cost to the ith control point of the velocity trajectory.
- AddPathLengthCost(self: pydrake.planning.KinematicTrajectoryOptimization, weight: float = 1.0, use_conic_constraint: bool = False) list[pydrake.solvers.Binding[Cost]]
Adds a cost on an upper bound of the length of the path, ∫₀ᵀ |q̇(t)|₂ dt, or equivalently ∫₀¹ |ṙ(s)|₂ ds, by summing the distance between the path control points. If
use_conic_constraint = false
, then costs are added via MathematicalProgram::AddL2NormCost; otherwise they are added via MathematicalProgram::AddL2NormCostUsingConicConstraint.- Returns
A vector of bindings with the ith element adding a cost to the ith control point of the velocity trajectory.
- AddPathPositionConstraint(*args, **kwargs)
Overloaded function.
AddPathPositionConstraint(self: pydrake.planning.KinematicTrajectoryOptimization, lb: numpy.ndarray[numpy.float64[m, 1]], ub: numpy.ndarray[numpy.float64[m, 1]], s: float) -> pydrake.solvers.Binding[LinearConstraint]
Adds a linear constraint on the value of the path,
lb
≤ r(s) ≤ub
.- Precondition:
0 <=
s
<= 1.
AddPathPositionConstraint(self: pydrake.planning.KinematicTrajectoryOptimization, constraint: pydrake.solvers.Constraint, s: float) -> pydrake.solvers.Binding[Constraint]
Adds a (generic) constraint on path. The constraint will be evaluated as if it is bound with variables corresponding to
r(s)
.- Precondition:
constraint.num_vars() == num_positions()
- Precondition:
0 <=
s
<= 1.
- AddPathVelocityConstraint(self: pydrake.planning.KinematicTrajectoryOptimization, lb: numpy.ndarray[numpy.float64[m, 1]], ub: numpy.ndarray[numpy.float64[m, 1]], s: float) pydrake.solvers.Binding[LinearConstraint]
Adds a linear constraint on the derivative of the path,
lb
≤ ṙ(s) ≤ub
. Note that this does NOT directly constrain q̇(t).- Precondition:
0 <=
s
<= 1.
- AddPositionBounds(self: pydrake.planning.KinematicTrajectoryOptimization, lb: numpy.ndarray[numpy.float64[m, 1]], ub: numpy.ndarray[numpy.float64[m, 1]]) list[pydrake.solvers.Binding[BoundingBoxConstraint]]
Adds bounding box constraints to enforce upper and lower bounds on the positions trajectory, q(t). These bounds will be respected at all times, t∈[0,T]. This also implies the constraints are satisfied for r(s), for all s∈[0,1].
- Returns
A vector of bindings with the ith element adding a constraint to the ith control point.
- AddVelocityBounds(self: pydrake.planning.KinematicTrajectoryOptimization, lb: numpy.ndarray[numpy.float64[m, 1]], ub: numpy.ndarray[numpy.float64[m, 1]]) list[pydrake.solvers.Binding[LinearConstraint]]
Adds linear constraints to enforce upper and lower bounds on the velocity trajectory, q̇(t). These bounds will be respected at all times. Note this does NOT directly constrain ṙ(s).
- Returns
A vector of bindings with the ith element adding a constraint to the ith control point of the derivative trajectory.
- AddVelocityConstraintAtNormalizedTime(self: pydrake.planning.KinematicTrajectoryOptimization, constraint: pydrake.solvers.Constraint, s: float) pydrake.solvers.Binding[Constraint]
Adds a (generic) constraint on trajectory velocity
q̇(t)
, evaluated ats
. The constraint will be evaluated as if it is bound with variables corresponding to[q(T*s), q̇(T*s)]
.This is a potentially confusing mix of
s
andt
, but it is important in practice. For instance if you want to constrain the true (trajectory) velocity at the final time, one would naturally want to write AddVelocityConstraint(constraint, s=1).This method should be compared with AddPathVelocityConstraint, which only constrains ṙ(s) because it does not reason about the time scaling, T. However, AddPathVelocityConstraint adds convex constraints, whereas this method adds nonconvex generic constraints.
- Precondition:
constraint.num_vars() == num_positions()
- Precondition:
0 <=
s
<= 1.
- basis(self: pydrake.planning.KinematicTrajectoryOptimization) pydrake.math.BsplineBasis
Returns the basis used to represent the path, r(s), over s∈[0,1].
- control_points(self: pydrake.planning.KinematicTrajectoryOptimization) numpy.ndarray[object[m, n]]
Returns the control points defining the path as an M-by-N matrix, where M is the number of positions and N is the number of control points.
- duration(self: pydrake.planning.KinematicTrajectoryOptimization) pydrake.symbolic.Variable
Returns the decision variable defining the time duration of the trajectory.
- get_mutable_prog(self: pydrake.planning.KinematicTrajectoryOptimization) pydrake.solvers.MathematicalProgram
Getter for a mutable pointer to the optimization program.
- num_control_points(self: pydrake.planning.KinematicTrajectoryOptimization) int
Returns the number of control points used for the path.
- num_positions(self: pydrake.planning.KinematicTrajectoryOptimization) int
Returns the number of position variables.
- prog(self: pydrake.planning.KinematicTrajectoryOptimization) pydrake.solvers.MathematicalProgram
Getter for the optimization program.
- ReconstructTrajectory(self: pydrake.planning.KinematicTrajectoryOptimization, result: pydrake.solvers.MathematicalProgramResult) pydrake.trajectories.BsplineTrajectory
Returns the trajectory q(t) from the
result
of solvingprog()
.
- SetInitialGuess(self: pydrake.planning.KinematicTrajectoryOptimization, trajectory: pydrake.trajectories.BsplineTrajectory) None
Sets the initial guess for the path and duration to match
trajectory
.- Precondition:
trajectory.rows() == num_positions()
- Precondition:
trajectory.columns() == 1
- class pydrake.planning.LinearDistanceAndInterpolationProvider
Bases:
pydrake.planning.DistanceAndInterpolationProvider
This class represents a basic “linear” implementation of DistanceAndInterpolationProvider.
Configuration distance is computed as difference.cwiseProduct(weights).norm(),
where difference is computed as the angle between quaternion DoF and difference between all other positions. Default weights are (1, 0, 0, 0) for quaternion DoF and 1 for all other positions. - Configuration interpolation is performed using slerp for quaternion DoF and linear interpolation for all other positions.
- __init__(*args, **kwargs)
Overloaded function.
__init__(self: pydrake.planning.LinearDistanceAndInterpolationProvider, plant: pydrake.multibody.plant.MultibodyPlant) -> None
Constructs a LinearDistanceAndInterpolationProvider for the specified
plant
with default distance weights (i.e. 1) for all positions. Equivalent to constructing with an empty map of named joint distance weights.__init__(self: pydrake.planning.LinearDistanceAndInterpolationProvider, plant: pydrake.multibody.plant.MultibodyPlant, distance_weights: numpy.ndarray[numpy.float64[m, 1]]) -> None
Constructs a LinearDistanceAndInterpolationProvider for the specified
plant
with the provided distance weightsdistance_weights
.- Precondition:
distance_weights must be the same size as plant.num_positions(), all weights must be non-negative and finite, and weights for quaternion DoF must be of the form (weight, 0, 0, 0).
__init__(self: pydrake.planning.LinearDistanceAndInterpolationProvider, plant: pydrake.multibody.plant.MultibodyPlant, joint_distance_weights: dict[pydrake.multibody.tree.JointIndex, numpy.ndarray[numpy.float64[m, 1]]]) -> None
Constructs a LinearDistanceAndInterpolationProvider for the specified
plant
using the provided map of distance weightsjoint_distance_weights
and default weights (i.e. 1) for all other positions.- Precondition:
all distance weights must be non-negative and finite.
- distance_weights(self: pydrake.planning.LinearDistanceAndInterpolationProvider) numpy.ndarray[numpy.float64[m, 1]]
Gets the distance weights.
- quaternion_dof_start_indices(self: pydrake.planning.LinearDistanceAndInterpolationProvider) list[int]
Gets the start indices for quaternion DoF in the position vector.
- pydrake.planning.MakeBodyShapeDescription(plant: pydrake.multibody.plant.MultibodyPlant, plant_context: pydrake.systems.framework.Context, geometry_id: pydrake.geometry.GeometryId) pydrake.planning.BodyShapeDescription
Constructs a BodyShapeDescription by extracting the shape, pose, and names associated with the provided geometry_id.
- Precondition:
plant_context
is compatible withplant
.- Precondition:
plant
is connected to a scene graph.- Precondition:
geometry_id
refers to a geometry rigidly affixed to a body ofplant
.
- class pydrake.planning.MaxCliqueSolverBase
The problem of finding the maximum clique in a graph is known to be NP-complete. This base class provides a unified interface for various implementations of a solver for this problem which may be solved rigorously or via heuristics.
- __init__(self: pydrake.planning.MaxCliqueSolverBase) None
- SolveMaxClique(self: pydrake.planning.MaxCliqueSolverBase, adjacency_matrix: scipy.sparse.csc_matrix[bool]) numpy.ndarray[bool[m, 1]]
Given the adjacency matrix of an undirected graph, find the maximum clique within the graph. A clique is a collection of vertices in a graph such that each pair of vertices is connected by an edge (i.e. a fully connected subgraph). This problem is known to be NP-complete, and so the concrete implementation of the solver determines whether the return of this function is the true maximum clique in the graph (which may take very long to compute), or only an approximate solution found via heuristics.
This method throws if the adjacency matrix is not symmetric and may throw depending on the concrete implementation of the solver.
- Parameter
adjacency_matrix
: a symmetric binary matrix encoding the edge relationship.
- Returns
A binary vector with the same indexing as the adjacency matrix, with true indicating membership in the clique.
- Parameter
- class pydrake.planning.MaxCliqueSolverViaGreedy
Bases:
pydrake.planning.MaxCliqueSolverBase
Approximately solves the maximum clique problem via a greedy heuristic. Vertices are greedily added to the clique based on their degree of connectivity. The algorithm initializes the clique with an empty set and makes every vertex a candidate, then the degree of every vertex is computed and the candidate vertex with the highest degree is added to the clique. Afterwards, new candidate list is updated and the previous two steps are repeated until no candidates are left.
- __init__(self: pydrake.planning.MaxCliqueSolverViaGreedy) None
- class pydrake.planning.MaxCliqueSolverViaMip
Bases:
pydrake.planning.MaxCliqueSolverBase
Solves the maximum clique problem to global optimality by solving the mixed-integer program
Maximize ∑ᵢ xᵢ subject to xᵢ + xⱼ ≤ 1 if (i,j) is not in the edge xᵢ ∈ {0,1}.
Note: This solver requires the availability of a Mixed-Integer Linear Programming solver (e.g. Gurobi and/or Mosek). We recommend enabling those solvers if possible (https://drake.mit.edu/bazel.html#proprietary_solvers).
- Raises
RuntimeError if no Mixed-Integer Linear Programming solver is –
available. –
RuntimeError if the initial guess has the wrong size for the –
provided adjacency matrix. –
- __init__(*args, **kwargs)
Overloaded function.
__init__(self: pydrake.planning.MaxCliqueSolverViaMip) -> None
__init__(self: pydrake.planning.MaxCliqueSolverViaMip, initial_guess: Optional[numpy.ndarray[numpy.float64[m, 1]]], solver_options: pydrake.solvers.SolverOptions) -> None
- GetInitialGuess(self: pydrake.planning.MaxCliqueSolverViaMip) Optional[numpy.ndarray[numpy.float64[m, 1]]]
- GetSolverOptions(self: pydrake.planning.MaxCliqueSolverViaMip) pydrake.solvers.SolverOptions
- SetInitialGuess(self: pydrake.planning.MaxCliqueSolverViaMip, initial_guess: Optional[numpy.ndarray[numpy.float64[m, 1]]]) None
- SetSolverOptions(self: pydrake.planning.MaxCliqueSolverViaMip, solver_options: pydrake.solvers.SolverOptions) None
- class pydrake.planning.MinCliqueCoverSolverBase
- __init__(self: pydrake.planning.MinCliqueCoverSolverBase) None
- SolveMinCliqueCover(self: pydrake.planning.MinCliqueCoverSolverBase, adjacency_matrix: scipy.sparse.csc_matrix[bool], partition: bool = False) list[set[int]]
Given the adjacency matrix of an undirected graph, finds a (potentially approximate) minimum clique cover of the graph. A clique is a collection of vertices in a graph such that each pair of vertices is connected by an edge (i.e. a fully connected subgraph). A clique cover is a collection of cliques where each vertex in the graph is in at least one of the cliques. This problem is known to be NP-complete, and so the concrete implementation of the solver determines whether the return of this function is the true minimum clique cover of the graph (which may take very long to compute), or only an approximate solution found via heuristics.
This method throws if the adjacency matrix is not symmetric and may throw depending on the concrete implementation of the solver.
- Parameter
adjacency_matrix
: a symmetric binary matrix encoding the edge relationship.
- Parameter
partition
: If true, then every vertex is allowed to be covered exactly once. Otherwise, the same vertex may be covered multiple times.
- Returns
A binary vector with the same indexing as the adjacency matrix, with true indicating membership in the clique.
- Returns
The ith entry of the returned vector is a vector containing the indices of the ith clique.
Note that this method is intentionally not-const because some solvers may use randomized algorithms.
- Parameter
- class pydrake.planning.MinCliqueCoverSolverViaGreedy
Bases:
pydrake.planning.MinCliqueCoverSolverBase
Approximately solves the min clique cover problem via a greedy heuristic.
At each step, the largest clique found by the provided max clique solver is added to the clique cover. These vertices are then removed from graph and max clique is solved again. This loop continues until a clique is found which is smaller than the min clique size.
Note
if min clique size > 1, then this class will not strictly compute a clique cover since not all vertices will be covered.
- __init__(self: pydrake.planning.MinCliqueCoverSolverViaGreedy, max_clique_solver: pydrake.planning.MaxCliqueSolverBase, min_clique_size: int = 1) None
- get_min_clique_size(self: pydrake.planning.MinCliqueCoverSolverViaGreedy) int
- set_min_clique_size(self: pydrake.planning.MinCliqueCoverSolverViaGreedy, min_clique_size: int) None
Set the minimum clique size. Throws if this is less than 1.
Parameter
min_clique_size
:
- class pydrake.planning.MultipleShooting
MultipleShooting is an abstract class for trajectory optimization that creates decision variables for inputs, states, and (optionally) sample times along the trajectory, then provides a number of methods for working with those decision variables.
MultipleShooting classes add decision variables, costs, and constraints to a MathematicalProgram. You can retrieve that program using prog(), and add additional variables, costs, and constraints using the MathematicalProgram interface directly.
Subclasses must implement the abstract methods: DoAddRunningCost() ReconstructInputTrajectory() ReconstructStateTrajectory() using all of the correct interpolation schemes for the specific transcription method, and should add the constraints to impose the System% dynamics in their constructor.
This class assumes that there are a fixed number (N) time steps/samples, and that the trajectory is discretized into time steps h (N-1 of these), state x (N of these), and control input u (N of these).
- __init__(*args, **kwargs)
- AddCompleteTrajectoryCallback(self: pydrake.planning.MultipleShooting, callback: Callable[[numpy.ndarray[numpy.float64[m, 1]], numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], list[numpy.ndarray[numpy.float64[m, n], flags.f_contiguous]]], None], names: list[str]) pydrake.solvers.Binding[VisualizationCallback]
Adds a callback method to visualize intermediate results of all variables used in the trajectory optimization. The callback should be of the form MyVisualization(sample_times, states, inputs, values), where sample_times is an N-by-1 VectorXd of sample times, states is a num_states-by-N MatrixXd of the current (intermediate) state trajectory at the break points, inputs is a num_inputs-by-N MatrixXd of the current (intermediate) input trajectory at the break points and values is a vector of num_rows-by-N MatrixXds of the current (intermediate) extra sequential variables specified by
names
at the break points.Note
Just like other costs/constraints, not all solvers support callbacks. Adding a callback here will force MathematicalProgram::Solve to select a solver that support callbacks. For instance, adding a visualization callback to a quadratic programming problem may result in using a nonlinear programming solver as the default solver.
- AddConstraintToAllKnotPoints(*args, **kwargs)
Overloaded function.
AddConstraintToAllKnotPoints(self: pydrake.planning.MultipleShooting, f: pydrake.symbolic.Formula) -> list[pydrake.solvers.Binding[Constraint]]
Adds a constraint to all breakpoints, where any instances of time(), state(), and/or input() placeholder variables, as well as placeholder variables returned by calls to NewSequentialVariable(), are substituted with the relevant variables for each time index.
- Returns
A vector of the bindings added to each knot point.
AddConstraintToAllKnotPoints(self: pydrake.planning.MultipleShooting, f: numpy.ndarray[object[m, 1]]) -> list[pydrake.solvers.Binding[Constraint]]
Variant of AddConstraintToAllKnotPoints that accepts a vector of formulas.
AddConstraintToAllKnotPoints(self: pydrake.planning.MultipleShooting, constraint: pydrake.solvers.BoundingBoxConstraint, vars: numpy.ndarray[object[m, 1]]) -> list[pydrake.solvers.Binding[BoundingBoxConstraint]]
Adds a constraint to all breakpoints, where any instances in
vars
of time(), state(), and/or input() placeholder variables, as well as placeholder variables returned by calls to NewSequentialVariable(), are substituted with the relevant variables for each time index.- Returns
A vector of the bindings added to each knot point.
AddConstraintToAllKnotPoints(self: pydrake.planning.MultipleShooting, constraint: pydrake.solvers.LinearEqualityConstraint, vars: numpy.ndarray[object[m, 1]]) -> list[pydrake.solvers.Binding[LinearEqualityConstraint]]
Adds a constraint to all breakpoints, where any instances in
vars
of time(), state(), and/or input() placeholder variables, as well as placeholder variables returned by calls to NewSequentialVariable(), are substituted with the relevant variables for each time index.- Returns
A vector of the bindings added to each knot point.
AddConstraintToAllKnotPoints(self: pydrake.planning.MultipleShooting, constraint: pydrake.solvers.LinearConstraint, vars: numpy.ndarray[object[m, 1]]) -> list[pydrake.solvers.Binding[LinearConstraint]]
Adds a constraint to all breakpoints, where any instances in
vars
of time(), state(), and/or input() placeholder variables, as well as placeholder variables returned by calls to NewSequentialVariable(), are substituted with the relevant variables for each time index.- Returns
A vector of the bindings added to each knot point.
AddConstraintToAllKnotPoints(self: pydrake.planning.MultipleShooting, constraint: pydrake.solvers.Constraint, vars: numpy.ndarray[object[m, 1]]) -> list[pydrake.solvers.Binding[Constraint]]
Adds a constraint to all breakpoints, where any instances in
vars
of time(), state(), and/or input() placeholder variables, as well as placeholder variables returned by calls to NewSequentialVariable(), are substituted with the relevant variables for each time index.- Returns
A vector of the bindings added to each knot point.
- AddDurationBounds(self: pydrake.planning.MultipleShooting, lower_bound: float, upper_bound: float) pydrake.solvers.Binding[LinearConstraint]
Adds a constraint on the total duration of the trajectory.
- Parameter
lower_bound
: A scalar double lower bound.
- Parameter
upper_bound
: A scalar double upper bound.
- Returns
The constraint enforcing the duration bounds.
- Raises
RuntimeError if time steps are not declared as decision variables. –
- Parameter
- AddEqualTimeIntervalsConstraints(self: pydrake.planning.MultipleShooting) list[pydrake.solvers.Binding[LinearConstraint]]
Adds constraints to enforce that all time steps have equal duration.
- Returns
A vector of constraints enforcing all time intervals are equal.
- Raises
RuntimeError if time steps are not declared as decision variables. –
- AddFinalCost(*args, **kwargs)
Overloaded function.
AddFinalCost(self: pydrake.planning.MultipleShooting, e: pydrake.symbolic.Expression) -> pydrake.solvers.Binding[Cost]
Adds a cost to the final time, of the form
\[cost = e(t,x,u),\]where any instances of time(), state(), and/or input() placeholder variables, as well as placeholder variables returned by calls to NewSequentialVariable(), are substituted with the relevant variables for the final time index.
- Returns
The final cost added to the problem.
AddFinalCost(self: pydrake.planning.MultipleShooting, matrix: numpy.ndarray[object[m, n], flags.f_contiguous]) -> pydrake.solvers.Binding[Cost]
Adds support for passing in a (scalar) matrix Expression, which is a common output of most symbolic linear algebra operations.
- Returns
The final cost added to the problem.
Note
Derived classes will need to type using MultipleShooting::AddFinalCost; to “unhide” this method.
- AddInputTrajectoryCallback(self: pydrake.planning.MultipleShooting, callback: Callable[[numpy.ndarray[numpy.float64[m, 1]], numpy.ndarray[numpy.float64[m, n], flags.f_contiguous]], None]) pydrake.solvers.Binding[VisualizationCallback]
Adds a callback method to visualize intermediate results of input variables used in the trajectory optimization. The callback should be of the form MyVisualization(sample_times, values), where breaks is a N-by-1 VectorXd of sample times, and values is a num_inputs-by-N MatrixXd representing the current (intermediate) value of the input trajectory at the break points in each column.
Note
Just like other costs/constraints, not all solvers support callbacks. Adding a callback here will force MathematicalProgram::Solve to select a solver that support callbacks. For instance, adding a visualization callback to a quadratic programming problem may result in using a nonlinear programming solver as the default solver.
- AddRunningCost(*args, **kwargs)
Overloaded function.
AddRunningCost(self: pydrake.planning.MultipleShooting, g: pydrake.symbolic.Expression) -> None
Adds an integrated cost to all time steps, of the form
\[cost = \int_0^T g(t,x,u) dt,\]where any instances of time(), state(), and/or input() placeholder variables, as well as placeholder variables returned by calls to NewSequentialVariable(), are substituted with the relevant variables for each time index. The particular integration scheme is determined by the derived class implementation.
AddRunningCost(self: pydrake.planning.MultipleShooting, g: numpy.ndarray[object[m, n], flags.f_contiguous]) -> None
Adds support for passing in a (scalar) matrix Expression, which is a common output of most symbolic linear algebra operations.
- AddStateTrajectoryCallback(self: pydrake.planning.MultipleShooting, callback: Callable[[numpy.ndarray[numpy.float64[m, 1]], numpy.ndarray[numpy.float64[m, n], flags.f_contiguous]], None]) pydrake.solvers.Binding[VisualizationCallback]
Adds a callback method to visualize intermediate results of state variables used in the trajectory optimization. The callback should be of the form MyVisualization(sample_times, values), where sample_times is a N-by-1 VectorXd of sample times, and values is a num_states-by-N MatrixXd representing the current (intermediate) value of the state trajectory at the break points in each column.
Note
Just like other costs/constraints, not all solvers support callbacks. Adding a callback here will force MathematicalProgram::Solve to select a solver that support callbacks. For instance, adding a visualization callback to a quadratic programming problem may result in using a nonlinear programming solver as the default solver.
- AddTimeIntervalBounds(self: pydrake.planning.MultipleShooting, lower_bound: float, upper_bound: float) pydrake.solvers.Binding[BoundingBoxConstraint]
Adds bounds on all time intervals.
- Parameter
lower_bound
: A scalar double lower bound.
- Parameter
upper_bound
: A scalar double upper bound.
- Returns
The bounding box constraint enforcing time interval bounds.
- Raises
RuntimeError if time steps are not declared as decision variables. –
- Parameter
- final_state(self: pydrake.planning.MultipleShooting) numpy.ndarray[object[m, 1]]
Returns the decision variables associated with the state, x, at the final time index.
- fixed_time_step(self: pydrake.planning.MultipleShooting) float
- GetInputSamples(self: pydrake.planning.MultipleShooting, result: pydrake.solvers.MathematicalProgramResult) numpy.ndarray[numpy.float64[m, n]]
Returns a matrix containing the input values (arranged in columns) at each breakpoint at the solution.
- GetSampleTimes(self: pydrake.planning.MultipleShooting, result: pydrake.solvers.MathematicalProgramResult) numpy.ndarray[numpy.float64[m, 1]]
Returns a vector containing the elapsed time at each breakpoint.
- GetSequentialVariableAtIndex(self: pydrake.planning.MultipleShooting, name: str, index: int) numpy.ndarray[object[m, 1]]
Returns the decision variables associated with the sequential variable
name
at time indexindex
.See also
NewSequentialVariable().
- GetSequentialVariableSamples(self: pydrake.planning.MultipleShooting, result: pydrake.solvers.MathematicalProgramResult, name: str) numpy.ndarray[numpy.float64[m, n]]
Returns a matrix containing the sequential variable values (arranged in columns) at each breakpoint at the solution.
- Parameter
name
: The name of sequential variable to get the results for. Must correspond to an already added sequential variable.
- Parameter
- GetStateSamples(self: pydrake.planning.MultipleShooting, result: pydrake.solvers.MathematicalProgramResult) numpy.ndarray[numpy.float64[m, n]]
Returns a matrix containing the state values (arranged in columns) at each breakpoint at the solution.
- initial_state(self: pydrake.planning.MultipleShooting) numpy.ndarray[object[m, 1]]
Returns the decision variables associated with the state, x, at the initial time index.
- input(*args, **kwargs)
Overloaded function.
input(self: pydrake.planning.MultipleShooting) -> numpy.ndarray[object[m, 1]]
Returns placeholder decision variables (not actually declared as decision variables in the MathematicalProgram) associated with the input, u, but with the time-index undetermined. These variables will be substituted for real decision variables at particular times in methods like AddRunningCost. Passing these variables directly into objectives/constraints for the parent classes will result in an error.
input(self: pydrake.planning.MultipleShooting, index: int) -> numpy.ndarray[object[m, 1]]
Returns the decision variables associated with the input, u, at time index
index
.
- NewSequentialVariable(self: pydrake.planning.MultipleShooting, rows: int, name: str) numpy.ndarray[object[m, 1]]
Adds a sequential variable (a variable that has associated decision variables for each time index) to the optimization problem and returns a placeholder variable (not actually declared as a decision variable in the MathematicalProgram). This variable will be substituted for real decision variables at particular times in methods like AddRunningCost(). Passing this variable directly into objectives/constraints for the parent classes will result in an error.
- prog(self: pydrake.planning.MultipleShooting) pydrake.solvers.MathematicalProgram
Returns a reference to the MathematicalProgram associated with the trajectory optimization problem.
- ReconstructInputTrajectory(self: pydrake.planning.MultipleShooting, result: pydrake.solvers.MathematicalProgramResult) pydrake.trajectories.PiecewisePolynomial
Get the input trajectory at the solution as a PiecewisePolynomial. The order of the trajectory will be determined by the integrator used in the dynamic constraints. Requires that the system has at least one input port.
- ReconstructStateTrajectory(self: pydrake.planning.MultipleShooting, result: pydrake.solvers.MathematicalProgramResult) pydrake.trajectories.PiecewisePolynomial
Get the state trajectory at the solution as a PiecewisePolynomial. The order of the trajectory will be determined by the integrator used in the dynamic constraints.
- SetInitialTrajectory(self: pydrake.planning.MultipleShooting, traj_init_u: pydrake.trajectories.PiecewisePolynomial, traj_init_x: pydrake.trajectories.PiecewisePolynomial) None
- state(*args, **kwargs)
Overloaded function.
state(self: pydrake.planning.MultipleShooting) -> numpy.ndarray[object[m, 1]]
Returns placeholder decision variables (not actually declared as decision variables in the MathematicalProgram) associated with the state, x, but with the time-index undetermined. These variables will be substituted for real decision variables at particular times in methods like AddRunningCost. Passing these variables directly into objectives/constraints for the parent classes will result in an error.
state(self: pydrake.planning.MultipleShooting, index: int) -> numpy.ndarray[object[m, 1]]
Returns the decision variables associated with the state, x, at time index
index
.
- time(self: pydrake.planning.MultipleShooting) numpy.ndarray[object[1, 1]]
Returns a placeholder decision variable (not actually declared as a decision variable in the MathematicalProgram) associated with the time, t. This variable will be substituted for real decision variables at particular times in methods like AddRunningCost. Passing this variable directly into objectives/constraints for the parent classes will result in an error.
- time_step(self: pydrake.planning.MultipleShooting, index: int) numpy.ndarray[object[1, 1]]
Returns the decision variable associated with the time step, h, at time index
index
.- Raises
RuntimeError if time steps are not declared as decision variables. –
- class pydrake.planning.RobotClearance
A summary of the clearance – a collection of distance measurements – between the robot and everything in the world. This data can be used to define collision avoidance strategies.
Conceptually, this class represents a table:
body index R | body index O | type | ϕᴼ(R) | Jq_ϕᴼ(R) | |- ———-
- :———-: | :–: | :—: | :——: | | … | … |
Member functions return each column of the table as an ordered collection. The iᵗʰ entry in each collection belongs to the iᵗʰ row.
Each row characterizes the relationship between a particular robot body (annotated as body R) and some “other” body (annotated as body O) in the model. That other body O may be part of the robot or the environment.
body index R
is the BodyIndex of body R.body index O
is the BodyIndex of body O.type
implies the type of body O. Given that we know body R is a robot
body,
type
indicates that body O is also a robot body with the value RobotCollisionType::kSelfCollision or an environment body with the value RobotCollisionType::kEnvironmentCollision. For a correct implementation of CollisionChecker, it will never be RobotCollisionType::kEnvironmentAndSelfCollision. -ϕᴼ(R)
is the signed distance function of the other body O evaluated on body R. The reported distance is offset by the padding value for the body pair recorded in the CollisionChecker. It is the minimum padded distance between bodies O and R. A point on the padded surface of body O would report a distance of zero. Points inside that boundary report negative distance and points outside have positive distance. -Jqᵣ_ϕᴼ(R)
is the Jacobian of ϕᴼ(R) with respect to the robot configuration vectorqᵣ
. The Jacobian is the derivative as observed in the world frame. - The vectorqᵣ
will be a subset of the plant’s full configurationq
when there are floating bodies or joints in the plant other than the robot. The Jacobian is only taken with respect to the robot. - Thejacobians()
matrix hasplant.num_positions()
columns and the column order matches up with the fullplant.GetPositions()
order. The columns associated with non-robot joints will be zero.Several important notes:
A single robot body index can appear many times as there may be many
measured distances between that robot body and other bodies in the model. - A row may contain a zero-valued Jacobian Jqᵣ_ϕᴼ(R). Appropriately filtering collisions will cull most of these Jacobians. But depending on the structure of the robot and its representative collision geometry, it is still possible to evaluate the Jacobian at a configuration that represents a local optimum (zero-valued Jacobian).
- __init__(*args, **kwargs)
Overloaded function.
__init__(self: pydrake.planning.RobotClearance, num_positions: int) -> None
Creates an empty clearance with size() == 0 and num_positions as given.
__init__(self: pydrake.planning.RobotClearance, other: pydrake.planning.RobotClearance) -> None
- Append(self: pydrake.planning.RobotClearance, robot_index: pydrake.multibody.tree.BodyIndex, other_index: pydrake.multibody.tree.BodyIndex, collision_type: pydrake.planning.RobotCollisionType, distance: float, jacobian: numpy.ndarray[numpy.float64[1, n]]) None
Appends one measurement to this table.
- collision_types(self: pydrake.planning.RobotClearance) list[pydrake.planning.RobotCollisionType]
- Returns
the vector of body collision types.
- distances(self: pydrake.planning.RobotClearance) numpy.ndarray[numpy.float64[m, 1]]
- Returns
the vector of distances (
ϕᴼ(R)
).
- jacobians(self: pydrake.planning.RobotClearance) numpy.ndarray[numpy.float64[m, n], flags.c_contiguous]
- Returns
the vector of distance Jacobians (
Jqᵣ_ϕᴼ(R)
); the return type is a readonly Eigen::Map with size() rows and num_positions() columns.
- mutable_jacobians(self: pydrake.planning.RobotClearance) numpy.ndarray[numpy.float64[m, n], flags.writeable, flags.c_contiguous]
(Advanced) The mutable flavor of jacobians().
- num_positions(self: pydrake.planning.RobotClearance) int
- Returns
the number of positions (i.e., columns) in jacobians().
- other_indices(self: pydrake.planning.RobotClearance) list[pydrake.multibody.tree.BodyIndex]
- Returns
the vector of other body indices.
- Reserve(self: pydrake.planning.RobotClearance, size: int) None
Ensures this object has storage for at least
size
rows.
- robot_indices(self: pydrake.planning.RobotClearance) list[pydrake.multibody.tree.BodyIndex]
- Returns
the vector of robot body indices.
- size(self: pydrake.planning.RobotClearance) int
- Returns
the number of distance measurements (rows in the table).
- class pydrake.planning.RobotCollisionType
Enumerates these predicates (and their combinations): - is the robot in collision with itself? - is the robot in collision with something in the environment?
Members:
kNoCollision :
kEnvironmentCollision :
kSelfCollision :
kEnvironmentAndSelfCollision :
- __init__(self: pydrake.planning.RobotCollisionType, value: int) None
- kEnvironmentAndSelfCollision = <RobotCollisionType.kEnvironmentAndSelfCollision: 3>
- kEnvironmentCollision = <RobotCollisionType.kEnvironmentCollision: 1>
- kNoCollision = <RobotCollisionType.kNoCollision: 0>
- kSelfCollision = <RobotCollisionType.kSelfCollision: 2>
- MakeUpdated(self: pydrake.planning.RobotCollisionType, *, in_environment_collision: Optional[bool] = None, in_self_collision: Optional[bool] = None) pydrake.planning.RobotCollisionType
Returns the RobotCollisionType value that reflects an incrementally different collision status versus the current value; the
self
object is unchanged. Ifin_environment_collision
and/orin_self_collision
is given, the return value will reflect those given value(s). This function subsumes the C++ free functionsSetInEnvironmentCollision()
andSetInSelfCollision()
.
- property name
- property value
- class pydrake.planning.RobotDiagram
Bases:
pydrake.systems.framework.Diagram
Storage for a combined diagram, plant, and scene graph.
This class is a convenient syntactic sugar, especially in C++ code where it simplifies object lifetime tracking and downcasting of the plant and scene graph references. It’s purpose is to serve as planner-specific syntactic sugar for operating on a MultibodyPlant. For other purposes (e.g., simulation), users should generally prefer to just use a Diagram, instead.
Use RobotDiagramBuilder to construct a RobotDiagram.
By default, the ports exposed by a RobotDiagram are the set of all ports provided by the plant and scene graph (excluding the internal connections between the two). Refer to their individual overview figures for details (see multibody::MultibodyPlant and geometry::SceneGraph), or see the full list by viewing the robot_diagram.GetGraphvizString().
plant_actuation→ plant_applied_generalized_force→ ... etc ...→ RobotDiagram → plant_state → ... etc ... → scene_graph_query However, if the RobotDiagramBuilder::builder() was used to change the diagram or if either the plant or scene graph were renamed, then no ports will be exported by default. In that case, you can use the builder to export any desired ports.
Note
This class is templated; see
RobotDiagram_
for the list of instantiations.- __init__(*args, **kwargs)
- mutable_plant_context(self: pydrake.planning.RobotDiagram, root_context: pydrake.systems.framework.Context) pydrake.systems.framework.Context
Gets the contained plant’s context (mutable) out of the given root context. Refer to drake::systems::System::GetMyContextFromRoot() to understand
root_context
.- Raises
RuntimeError if the root_context is not a root context. –
- mutable_scene_graph(self: pydrake.planning.RobotDiagram) pydrake.geometry.SceneGraph
Gets the contained scene graph (mutable).
- mutable_scene_graph_context(self: pydrake.planning.RobotDiagram, root_context: pydrake.systems.framework.Context) pydrake.systems.framework.Context
Gets the contained scene graph’s context (mutable) out of the given root context. Refer to drake::systems::System::GetMyContextFromRoot() to understand
root_context
.- Raises
RuntimeError if the root_context is not a root context. –
- plant(self: pydrake.planning.RobotDiagram) pydrake.multibody.plant.MultibodyPlant
Gets the contained plant (readonly).
- plant_context(self: pydrake.planning.RobotDiagram, root_context: pydrake.systems.framework.Context) pydrake.systems.framework.Context
Gets the contained plant’s context (readonly) out of the given root context. Refer to drake::systems::System::GetMyContextFromRoot() to understand
root_context
.- Raises
RuntimeError if the root_context is not a root context. –
- scene_graph(self: pydrake.planning.RobotDiagram) pydrake.geometry.SceneGraph
Gets the contained scene graph (readonly).
- scene_graph_context(self: pydrake.planning.RobotDiagram, root_context: pydrake.systems.framework.Context) pydrake.systems.framework.Context
Gets the contained scene graph’s context (readonly) out of the given root context. Refer to drake::systems::System::GetMyContextFromRoot() to understand
root_context
.- Raises
RuntimeError if the root_context is not a root context. –
- template pydrake.planning.RobotDiagram_
Instantiations:
RobotDiagram_[float]
,RobotDiagram_[AutoDiffXd]
,RobotDiagram_[Expression]
- class pydrake.planning.RobotDiagram_[AutoDiffXd]
Bases:
pydrake.systems.framework.Diagram_[AutoDiffXd]
Storage for a combined diagram, plant, and scene graph.
This class is a convenient syntactic sugar, especially in C++ code where it simplifies object lifetime tracking and downcasting of the plant and scene graph references. It’s purpose is to serve as planner-specific syntactic sugar for operating on a MultibodyPlant. For other purposes (e.g., simulation), users should generally prefer to just use a Diagram, instead.
Use RobotDiagramBuilder to construct a RobotDiagram.
By default, the ports exposed by a RobotDiagram are the set of all ports provided by the plant and scene graph (excluding the internal connections between the two). Refer to their individual overview figures for details (see multibody::MultibodyPlant and geometry::SceneGraph), or see the full list by viewing the robot_diagram.GetGraphvizString().
plant_actuation→ plant_applied_generalized_force→ ... etc ...→ RobotDiagram → plant_state → ... etc ... → scene_graph_query However, if the RobotDiagramBuilder::builder() was used to change the diagram or if either the plant or scene graph were renamed, then no ports will be exported by default. In that case, you can use the builder to export any desired ports.
- __init__(*args, **kwargs)
- mutable_plant_context(self: pydrake.planning.RobotDiagram_[AutoDiffXd], root_context: pydrake.systems.framework.Context_[AutoDiffXd]) pydrake.systems.framework.Context_[AutoDiffXd]
Gets the contained plant’s context (mutable) out of the given root context. Refer to drake::systems::System::GetMyContextFromRoot() to understand
root_context
.- Raises
RuntimeError if the root_context is not a root context. –
- mutable_scene_graph(self: pydrake.planning.RobotDiagram_[AutoDiffXd]) pydrake.geometry.SceneGraph_[AutoDiffXd]
Gets the contained scene graph (mutable).
- mutable_scene_graph_context(self: pydrake.planning.RobotDiagram_[AutoDiffXd], root_context: pydrake.systems.framework.Context_[AutoDiffXd]) pydrake.systems.framework.Context_[AutoDiffXd]
Gets the contained scene graph’s context (mutable) out of the given root context. Refer to drake::systems::System::GetMyContextFromRoot() to understand
root_context
.- Raises
RuntimeError if the root_context is not a root context. –
- plant(self: pydrake.planning.RobotDiagram_[AutoDiffXd]) pydrake.multibody.plant.MultibodyPlant_[AutoDiffXd]
Gets the contained plant (readonly).
- plant_context(self: pydrake.planning.RobotDiagram_[AutoDiffXd], root_context: pydrake.systems.framework.Context_[AutoDiffXd]) pydrake.systems.framework.Context_[AutoDiffXd]
Gets the contained plant’s context (readonly) out of the given root context. Refer to drake::systems::System::GetMyContextFromRoot() to understand
root_context
.- Raises
RuntimeError if the root_context is not a root context. –
- scene_graph(self: pydrake.planning.RobotDiagram_[AutoDiffXd]) pydrake.geometry.SceneGraph_[AutoDiffXd]
Gets the contained scene graph (readonly).
- scene_graph_context(self: pydrake.planning.RobotDiagram_[AutoDiffXd], root_context: pydrake.systems.framework.Context_[AutoDiffXd]) pydrake.systems.framework.Context_[AutoDiffXd]
Gets the contained scene graph’s context (readonly) out of the given root context. Refer to drake::systems::System::GetMyContextFromRoot() to understand
root_context
.- Raises
RuntimeError if the root_context is not a root context. –
- class pydrake.planning.RobotDiagram_[Expression]
Bases:
pydrake.systems.framework.Diagram_[Expression]
Storage for a combined diagram, plant, and scene graph.
This class is a convenient syntactic sugar, especially in C++ code where it simplifies object lifetime tracking and downcasting of the plant and scene graph references. It’s purpose is to serve as planner-specific syntactic sugar for operating on a MultibodyPlant. For other purposes (e.g., simulation), users should generally prefer to just use a Diagram, instead.
Use RobotDiagramBuilder to construct a RobotDiagram.
By default, the ports exposed by a RobotDiagram are the set of all ports provided by the plant and scene graph (excluding the internal connections between the two). Refer to their individual overview figures for details (see multibody::MultibodyPlant and geometry::SceneGraph), or see the full list by viewing the robot_diagram.GetGraphvizString().
plant_actuation→ plant_applied_generalized_force→ ... etc ...→ RobotDiagram → plant_state → ... etc ... → scene_graph_query However, if the RobotDiagramBuilder::builder() was used to change the diagram or if either the plant or scene graph were renamed, then no ports will be exported by default. In that case, you can use the builder to export any desired ports.
- __init__(*args, **kwargs)
- mutable_plant_context(self: pydrake.planning.RobotDiagram_[Expression], root_context: pydrake.systems.framework.Context_[Expression]) pydrake.systems.framework.Context_[Expression]
Gets the contained plant’s context (mutable) out of the given root context. Refer to drake::systems::System::GetMyContextFromRoot() to understand
root_context
.- Raises
RuntimeError if the root_context is not a root context. –
- mutable_scene_graph(self: pydrake.planning.RobotDiagram_[Expression]) pydrake.geometry.SceneGraph_[Expression]
Gets the contained scene graph (mutable).
- mutable_scene_graph_context(self: pydrake.planning.RobotDiagram_[Expression], root_context: pydrake.systems.framework.Context_[Expression]) pydrake.systems.framework.Context_[Expression]
Gets the contained scene graph’s context (mutable) out of the given root context. Refer to drake::systems::System::GetMyContextFromRoot() to understand
root_context
.- Raises
RuntimeError if the root_context is not a root context. –
- plant(self: pydrake.planning.RobotDiagram_[Expression]) pydrake.multibody.plant.MultibodyPlant_[Expression]
Gets the contained plant (readonly).
- plant_context(self: pydrake.planning.RobotDiagram_[Expression], root_context: pydrake.systems.framework.Context_[Expression]) pydrake.systems.framework.Context_[Expression]
Gets the contained plant’s context (readonly) out of the given root context. Refer to drake::systems::System::GetMyContextFromRoot() to understand
root_context
.- Raises
RuntimeError if the root_context is not a root context. –
- scene_graph(self: pydrake.planning.RobotDiagram_[Expression]) pydrake.geometry.SceneGraph_[Expression]
Gets the contained scene graph (readonly).
- scene_graph_context(self: pydrake.planning.RobotDiagram_[Expression], root_context: pydrake.systems.framework.Context_[Expression]) pydrake.systems.framework.Context_[Expression]
Gets the contained scene graph’s context (readonly) out of the given root context. Refer to drake::systems::System::GetMyContextFromRoot() to understand
root_context
.- Raises
RuntimeError if the root_context is not a root context. –
- class pydrake.planning.RobotDiagramBuilder
Storage for a combined diagram builder, plant, and scene graph. When T == double, a parser (and package map) is also available.
This class is a convenient syntactic sugar to help build a robot diagram, especially in C++ code where it simplifies object lifetime tracking and downcasting of the plant and scene graph references.
Note
This class is templated; see
RobotDiagramBuilder_
for the list of instantiations.- __init__(self: pydrake.planning.RobotDiagramBuilder, time_step: float = 0.001) None
Constructs with the specified time step for the contained plant.
- Parameter
time_step
: Governs whether the MultibodyPlant is modeled as a discrete system (
time_step > 0
) or as a continuous system (time_step = 0
). See multibody_simulation for further details. The default here matches the default value from multibody::MultibodyPlantConfig.
- Parameter
- Build(self: pydrake.planning.RobotDiagramBuilder) pydrake.planning.RobotDiagram
Builds the diagram and returns the diagram plus plant and scene graph in a RobotDiagram. The plant will be finalized during this function, unless it’s already been finalized.
- Raises
exception when IsDiagramBuilt() already. –
- builder(self: pydrake.planning.RobotDiagramBuilder) pydrake.systems.framework.DiagramBuilder
Gets the contained DiagramBuilder (mutable). Do not call Build() on the return value; instead, call Build() on this.
- Raises
exception when IsDiagramBuilt() already. –
- IsDiagramBuilt(self: pydrake.planning.RobotDiagramBuilder) bool
Checks if the diagram has already been built.
- parser(self: pydrake.planning.RobotDiagramBuilder) pydrake.multibody.parsing.Parser
Gets the contained Parser (mutable).
- Raises
exception when IsDiagramBuilt() already. –
- plant(self: pydrake.planning.RobotDiagramBuilder) pydrake.multibody.plant.MultibodyPlant
Gets the contained plant (mutable).
- Raises
exception when IsDiagramBuilt() already. –
- scene_graph(self: pydrake.planning.RobotDiagramBuilder) pydrake.geometry.SceneGraph
Gets the contained scene graph (mutable).
- Raises
exception when IsDiagramBuilt() already. –
- template pydrake.planning.RobotDiagramBuilder_
Instantiations:
RobotDiagramBuilder_[float]
,RobotDiagramBuilder_[AutoDiffXd]
,RobotDiagramBuilder_[Expression]
- class pydrake.planning.RobotDiagramBuilder_[AutoDiffXd]
Storage for a combined diagram builder, plant, and scene graph. When T == double, a parser (and package map) is also available.
This class is a convenient syntactic sugar to help build a robot diagram, especially in C++ code where it simplifies object lifetime tracking and downcasting of the plant and scene graph references.
- __init__(self: pydrake.planning.RobotDiagramBuilder_[AutoDiffXd], time_step: float = 0.001) None
Constructs with the specified time step for the contained plant.
- Parameter
time_step
: Governs whether the MultibodyPlant is modeled as a discrete system (
time_step > 0
) or as a continuous system (time_step = 0
). See multibody_simulation for further details. The default here matches the default value from multibody::MultibodyPlantConfig.
- Parameter
- Build(self: pydrake.planning.RobotDiagramBuilder_[AutoDiffXd]) pydrake.planning.RobotDiagram_[AutoDiffXd]
Builds the diagram and returns the diagram plus plant and scene graph in a RobotDiagram. The plant will be finalized during this function, unless it’s already been finalized.
- Raises
exception when IsDiagramBuilt() already. –
- builder(self: pydrake.planning.RobotDiagramBuilder_[AutoDiffXd]) pydrake.systems.framework.DiagramBuilder_[AutoDiffXd]
Gets the contained DiagramBuilder (mutable). Do not call Build() on the return value; instead, call Build() on this.
- Raises
exception when IsDiagramBuilt() already. –
- IsDiagramBuilt(self: pydrake.planning.RobotDiagramBuilder_[AutoDiffXd]) bool
Checks if the diagram has already been built.
- plant(self: pydrake.planning.RobotDiagramBuilder_[AutoDiffXd]) pydrake.multibody.plant.MultibodyPlant_[AutoDiffXd]
Gets the contained plant (mutable).
- Raises
exception when IsDiagramBuilt() already. –
- scene_graph(self: pydrake.planning.RobotDiagramBuilder_[AutoDiffXd]) pydrake.geometry.SceneGraph_[AutoDiffXd]
Gets the contained scene graph (mutable).
- Raises
exception when IsDiagramBuilt() already. –
- class pydrake.planning.RobotDiagramBuilder_[Expression]
Storage for a combined diagram builder, plant, and scene graph. When T == double, a parser (and package map) is also available.
This class is a convenient syntactic sugar to help build a robot diagram, especially in C++ code where it simplifies object lifetime tracking and downcasting of the plant and scene graph references.
- __init__(self: pydrake.planning.RobotDiagramBuilder_[Expression], time_step: float = 0.001) None
Constructs with the specified time step for the contained plant.
- Parameter
time_step
: Governs whether the MultibodyPlant is modeled as a discrete system (
time_step > 0
) or as a continuous system (time_step = 0
). See multibody_simulation for further details. The default here matches the default value from multibody::MultibodyPlantConfig.
- Parameter
- Build(self: pydrake.planning.RobotDiagramBuilder_[Expression]) pydrake.planning.RobotDiagram_[Expression]
Builds the diagram and returns the diagram plus plant and scene graph in a RobotDiagram. The plant will be finalized during this function, unless it’s already been finalized.
- Raises
exception when IsDiagramBuilt() already. –
- builder(self: pydrake.planning.RobotDiagramBuilder_[Expression]) pydrake.systems.framework.DiagramBuilder_[Expression]
Gets the contained DiagramBuilder (mutable). Do not call Build() on the return value; instead, call Build() on this.
- Raises
exception when IsDiagramBuilt() already. –
- IsDiagramBuilt(self: pydrake.planning.RobotDiagramBuilder_[Expression]) bool
Checks if the diagram has already been built.
- plant(self: pydrake.planning.RobotDiagramBuilder_[Expression]) pydrake.multibody.plant.MultibodyPlant_[Expression]
Gets the contained plant (mutable).
- Raises
exception when IsDiagramBuilt() already. –
- scene_graph(self: pydrake.planning.RobotDiagramBuilder_[Expression]) pydrake.geometry.SceneGraph_[Expression]
Gets the contained scene graph (mutable).
- Raises
exception when IsDiagramBuilt() already. –
- class pydrake.planning.SceneGraphCollisionChecker
Bases:
pydrake.planning.CollisionChecker
An implementation of CollisionChecker that uses SceneGraph to provide collision checks. Its behavior and limitations are exactly those of SceneGraph, e.g., in terms of what kinds of geometries can be collided.
- __init__(self: pydrake.planning.SceneGraphCollisionChecker, *, model: pydrake.planning.RobotDiagram, **kwargs) None
Creates a new checker with the given params.
See
pydrake.planning.CollisionCheckerParams
for the list of properties available here as kwargs.
- class pydrake.planning.UnimplementedCollisionChecker
Bases:
pydrake.planning.CollisionChecker
A concrete collision checker implementation that throws an exception for every virtual function hook. This might be useful for unit testing or for deriving your own collision checker without providing for the full suite of operations.
- __init__(self: pydrake.planning.UnimplementedCollisionChecker, *, model: pydrake.planning.RobotDiagram, supports_parallel_checking: bool, **kwargs) None
Constructs a checker.
- Parameter
supports_parallel_checking
: will serve as the return value of the CollisionChecker::SupportsParallelChecking() function.
See
pydrake.planning.CollisionCheckerParams
for the list of properties available here as kwargs.- Parameter
- pydrake.planning.VisibilityGraph(checker: pydrake.planning.CollisionChecker, points: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], parallelize: pydrake.common.Parallelism = True) scipy.sparse.csc_matrix[bool]
Given some number of sampled points in the configuration space of
checker`'s plant(), computes the "visibility graph" -- two `points
have an edge between them if the line segment connecting them is collision free. See CollisionChecker documentation for more information on how edge collision checks are performed.Note that this method assumes the collision checker has symmetric behavior (i.e. checking edge (q1, q2) is the same as checking edge (q2, q1)). This is true for many collision checkers (e.g. those using LinearDistanceAndInterpolationProvider, which is the default), but some more complex spaces with non-linear interpolation (e.g. a Dubin’s car) are not symmetric.
If
parallelize
specifies more than one thread, then the CollisionCheckerParams::distance_and_interpolation_provider forchecker
must be implemented in C++, either by providing the C++ implementation directly directly or by using the default provider.- Returns
the adjacency matrix, A(i,j) == true iff points.col(i) is visible from points.col(j). A is always symmetric.
- Precondition:
points.rows() == total number of positions in the collision checker plant.
- class pydrake.planning.ZmpPlanner
Given a desired two dimensional (X and Y) zero-moment point (ZMP) trajectory parameterized as a piecewise polynomial, an optimal center of mass (CoM) trajectory is planned using a linear inverted pendulum model (LIPM). A second order value function (optimal cost-to-go) and a linear policy are also computed alongside the optimal trajectory. The system dynamics for the X and Y directions are decoupled, however, we plan the XY motion together for convenience.
Let \(c\) be the CoM position, the state of the system, \(x\), is \([c; \dot{c}]\), the control, \(u = \ddot{c}\), and \(y\) represents the center of pressure (CoP). For the X direction, the LIPM dynamics is:
\[y = c - \frac{z}{g} * u,\]where \(g\) is the gravity constant and \(z\) is the CoM height. \(z\) is assumed to be constant in LIPM. The full dynamics can also be written in the matrix form as:
\[\dot{x} = A x + B u \\]y = C x + D u
The one step cost function \(L\) is defined as:
\[L(y, u, t) = (y - y_d(t))^T Q_y (y - y_d(t)) + u^T R u,\]where \(Q_y\) and \(R\) are weighting matrices, and \(y_d(t)\) is the desired ZMP trajectory at time \(t\).
The value function is defined as
\[V(x, t) = \min_{u[t:t_f]} \bar{x}(t_f)^T S \bar{x}(t_f)\]int_{t}^{t_f} L(y, u, tau) dtau,
subject to the dynamics, and \(t_f\) is the last time in the desired ZMP trajectory, \(\bar{x} = [c - y_d(t_f); \dot{c}]\), \(S\) is the quadratic term from the infinite horizon continuous time LQR solution solved with the same dynamics and one step cost function.
For this problem, \(V\) is known to have a quadratic form of:
\[V(x, t) = \bar{x}^T V_{xx} \bar{x} + \bar{x}^T V_x(t) + V_0(t),\]and the corresponding optimal control policy, \(u^*\), is linear w.r.t. to \(x\):
\[u^*(x, t) = K \bar{x} + u_0(t).\]See the following reference for more details about the algorithm:
[1] R. Tedrake, S. Kuindersma, R. Deits and K. Miura, “A closed-form solution for real-time ZMP gait generation and feedback stabilization,” 2015 IEEE-RAS 15th International Conference on Humanoid Robots (Humanoids), Seoul, 2015, pp. 936-940.
- __init__(self: pydrake.planning.ZmpPlanner) None
- comdd_to_cop(self: pydrake.planning.ZmpPlanner, x: numpy.ndarray[numpy.float64[4, 1]], u: numpy.ndarray[numpy.float64[2, 1]]) numpy.ndarray[numpy.float64[2, 1]]
Converts CoM acceleration to center of pressure (CoP) using cop = C * x + D * u, which is equivalent to cop = com - z / g * comdd Should only be called after Plan is called.
- Parameter
x
: CoM position and velocity
- Parameter
u
: CoM acceleration
- Returns
center of pressure (CoP)
- Precondition:
Plan() has already been called.
- Parameter
- ComputeOptimalCoMdd(self: pydrake.planning.ZmpPlanner, time: float, x: numpy.ndarray[numpy.float64[4, 1]]) numpy.ndarray[numpy.float64[2, 1]]
Computes the optimal control (CoM acceleration) at
time
given CoM statex
using the linear policy.- Parameter
time
: Current time.
- Parameter
x
: Current state.
- Returns
Optimal CoMdd.
- Precondition:
Plan() has already been called.
- Parameter
- get_A(self: pydrake.planning.ZmpPlanner) numpy.ndarray[numpy.float64[4, 4]]
Getter for A matrix.
- Precondition:
Plan() has already been called.
- get_B(self: pydrake.planning.ZmpPlanner) numpy.ndarray[numpy.float64[4, 2]]
Getter for B matrix.
- Precondition:
Plan() has already been called.
- get_C(self: pydrake.planning.ZmpPlanner) numpy.ndarray[numpy.float64[2, 4]]
Getter for C matrix.
- Precondition:
Plan() has already been called.
- get_D(self: pydrake.planning.ZmpPlanner) numpy.ndarray[numpy.float64[2, 2]]
Getter for D matrix.
- Precondition:
Plan() has already been called.
- get_desired_zmp(*args, **kwargs)
Overloaded function.
get_desired_zmp(self: pydrake.planning.ZmpPlanner, time: float) -> numpy.ndarray[numpy.float64[2, 1]]
Returns the desired ZMP evaluated at
time
.- Precondition:
Plan() has already been called.
get_desired_zmp(self: pydrake.planning.ZmpPlanner) -> pydrake.trajectories.PiecewisePolynomial
Returns the desired ZMP trajectory.
- Precondition:
Plan() has already been called.
- get_final_desired_zmp(self: pydrake.planning.ZmpPlanner) numpy.ndarray[numpy.float64[2, 1]]
Returns the position of the ZMP at the end of the desired trajectory.
- Precondition:
Plan() has already been called.
- get_nominal_com(*args, **kwargs)
Overloaded function.
get_nominal_com(self: pydrake.planning.ZmpPlanner, time: float) -> numpy.ndarray[numpy.float64[2, 1]]
Returns the nominal CoM position evaluated at
time
.- Precondition:
Plan() has already been called.
get_nominal_com(self: pydrake.planning.ZmpPlanner) -> pydrake.trajectories.ExponentialPlusPiecewisePolynomial
Returns the nominal CoM trajectory.
- Precondition:
Plan() has already been called.
- get_nominal_comd(*args, **kwargs)
Overloaded function.
get_nominal_comd(self: pydrake.planning.ZmpPlanner, time: float) -> numpy.ndarray[numpy.float64[2, 1]]
Returns the nominal CoM velocity evaluated at
time
.- Precondition:
Plan() has already been called.
get_nominal_comd(self: pydrake.planning.ZmpPlanner) -> pydrake.trajectories.ExponentialPlusPiecewisePolynomial
Returns the nominal CoM velocity trajectory.
- Precondition:
Plan() has already been called.
- get_nominal_comdd(*args, **kwargs)
Overloaded function.
get_nominal_comdd(self: pydrake.planning.ZmpPlanner, time: float) -> numpy.ndarray[numpy.float64[2, 1]]
Returns the nominal CoM acceleration evaluated at
time
.- Precondition:
Plan() has already been called.
get_nominal_comdd(self: pydrake.planning.ZmpPlanner) -> pydrake.trajectories.ExponentialPlusPiecewisePolynomial
Returns the nominal CoM acceleration trajectory.
- Precondition:
Plan() has already been called.
- get_Qy(self: pydrake.planning.ZmpPlanner) numpy.ndarray[numpy.float64[2, 2]]
Getter for Qy matrix.
- Precondition:
Plan() has already been called.
- get_R(self: pydrake.planning.ZmpPlanner) numpy.ndarray[numpy.float64[2, 2]]
Getter for R matrix.
- Precondition:
Plan() has already been called.
- get_Vx(*args, **kwargs)
Overloaded function.
get_Vx(self: pydrake.planning.ZmpPlanner, time: float) -> numpy.ndarray[numpy.float64[4, 1]]
Returns the time varying first order term (s2 in [1]) of the value function, evaluated at the given
time
.- Precondition:
Plan() has already been called.
get_Vx(self: pydrake.planning.ZmpPlanner) -> pydrake.trajectories.ExponentialPlusPiecewisePolynomial
Returns the time varying first order term (s2 in [1]) of the value function.
- Precondition:
Plan() has already been called.
- get_Vxx(self: pydrake.planning.ZmpPlanner) numpy.ndarray[numpy.float64[4, 4]]
Returns the time invariant second order term (S1 in [1]) of the value function.
- Precondition:
Plan() has already been called.
- has_planned(self: pydrake.planning.ZmpPlanner) bool
Returns true if Plan() has been called.
- Plan(self: pydrake.planning.ZmpPlanner, zmp_d: pydrake.trajectories.PiecewisePolynomial, x0: numpy.ndarray[numpy.float64[4, 1]], height: float, gravity: float = 9.81, Qy: numpy.ndarray[numpy.float64[2, 2]] = array([[1., 0.], [0., 1.]]), R: numpy.ndarray[numpy.float64[2, 2]] = array([[0.1, 0.], [0., 0.1]])) None