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.

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

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

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 class AllocateContexts() 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 provided parallelism 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.

AddCollisionShapes(*args, **kwargs)

Overloaded function.

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

2. 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 added shape 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.

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

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() reports SelfCollision) report one body’s index in robot_indices() and the the other body’s in other_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”.

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

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.

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

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.

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.

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

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

Clone(self: pydrake.planning.CollisionChecker) → pydrake.planning.CollisionChecker

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 and q2, 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 given body_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.

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

2. 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 and q2.

Parameter ratio:

Interpolation ratio.

Returns

Interpolated configuration.

Raises

RuntimeError if ratio is not in range [0, 1] –

See also

ConfigurationInterpolationFunction for more.

IsCollisionFilteredBetween(*args, **kwargs)

Overloaded function.

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

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

1. IsPartOfRobot(self: pydrake.planning.CollisionChecker, body: pydrake.multibody.tree.RigidBody) -> bool

Returns

true if the indicated body is part of the robot.

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

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

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.

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.

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 the context_number, or when nullopt the context to be used with the current OpenMP thread.

See also

ccb_implicit_contexts “Implicit Context Parallelism”.

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 the context_number, or when nullopt the context to be used with the current OpenMP thread.

See also

ccb_implicit_contexts “Implicit Context Parallelism”.

RemoveAllAddedCollisionShapes(*args, **kwargs)

Overloaded function.

1. RemoveAllAddedCollisionShapes(self: pydrake.planning.CollisionChecker, group_name: str) -> None

Removes all added checker geometries which belong to the named group.

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

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

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

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

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

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.

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.

1. 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``. –

2. 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 by context_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”.

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.

Clone(self: pydrake.planning.CollisionCheckerContext) → pydrake.planning.CollisionCheckerContext

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.

1. __init__(self: pydrake.planning.CollisionCheckerParams) -> None

2. __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 in context) or will be fixed to their current values (if they are connected/fixed in context).

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

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.

1. __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 in context) or will be set to their current values (if they are connected/fixed in context).

Default: kUseFirstInputIfItExists.

$Raises:

RuntimeError if context.has_only_discrete_state() == false.

2. __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 in context) or will be set to their current values (if they are connected/fixed in context).

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 configuration to. 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 and to at ratio.

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 q to qα can be considered collision free (where qα = interpolate(q1, q2, α)). partially_free() reports True. - α 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.

1. __init__(self: pydrake.planning.EdgeMeasure, distance: float, alpha: float) -> None

Precondition:

0 ≤ distance

Precondition:

0 ≤ alpha ≤ 1 to indicate defined alpha, negative otherwise.

2. __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() reports True, 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.

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. The GcsTrajectoryOptimization methods AddRegions and AddEdges 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.

AddEdges(self: pydrake.planning.GcsTrajectoryOptimization, from_subgraph: pydrake.planning.GcsTrajectoryOptimization.Subgraph, to_subgraph: pydrake.planning.GcsTrajectoryOptimization.Subgraph, subspace: pydrake.geometry.optimization.ConvexSet = 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.

AddPathContinuityConstraints(self: pydrake.planning.GcsTrajectoryOptimization, continuity_order: int) → None

Enforces derivative continuity constraints on the entire graph.

Parameter continuity_order:

is the order of the continuity constraint.

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.

Raises

• RuntimeError if the continuity order is less than one since path –

• continuity is enforced by default. –

AddPathLengthCost(*args, **kwargs)

Overloaded function.

1. 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().

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

1. 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. In other words, if defined, there must be one edge offset for each specified edge. For each pair of sets listed in edges_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.

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

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() –

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)

AddPathContinuityConstraints(self: pydrake.planning.GcsTrajectoryOptimization.EdgesBetweenSubgraphs, continuity_order: int) → None

Enforces derivative continuity constraints on the edges between the subgraphs.

Parameter continuity_order:

is the order of the continuity constraint.

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.

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

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() –

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

GetEdgesBetweenSubgraphs(self: pydrake.planning.GcsTrajectoryOptimization) → List[pydrake.planning.GcsTrajectoryOptimization.EdgesBetweenSubgraphs]

Returns a vector of all edges between subgraphs.

GetGraphvizString(self: pydrake.planning.GcsTrajectoryOptimization, result: Optional[pydrake.solvers.MathematicalProgramResult] = None, show_slack: bool = True, precision: int = 3, scientific: bool = False) → str

Returns a Graphviz string describing the graph vertices and edges. If results 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> –

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, solver_options=<SolverOptions>, rounding_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. –

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, solver_options=<SolverOptions>, rounding_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.

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)

AddPathContinuityConstraints(self: pydrake.planning.GcsTrajectoryOptimization.Subgraph, continuity_order: int) → None

Enforces derivative continuity constraints on the subgraph.

Parameter continuity_order:

is the order of the continuity constraint.

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.

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

AddPathLengthCost(*args, **kwargs)

Overloaded function.

1. 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().

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

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() –

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.

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 a planar joint

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

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.

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

2. __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 sets trajectory 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.

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 on length of the path, ∫₀ᵀ |q̇(t)|₂ dt, 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.

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

2. 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 at s. 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 and t, 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).

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 solving prog().

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.

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

2. __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 weights distance_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).

3. __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 weights joint_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 with plant.

Precondition:

plant is connected to a scene graph.

Precondition:

geometry_id refers to a geometry rigidly affixed to a body of plant.

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

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.

1. __init__(self: pydrake.planning.MaxCliqueSolverViaMip) -> None

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

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

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

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

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

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

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

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.

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

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

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

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

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

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.

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.

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

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

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

2. 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 vector qᵣ. The Jacobian is the derivative as observed in the world frame. - The vector qᵣ will be a subset of the plant’s full configuration q when there are floating bodies or joints in the plant other than the robot. The Jacobian is only taken with respect to the robot. - The jacobians() matrix has plant.num_positions() columns and the column order matches up with the full plant.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.

1. __init__(self: pydrake.planning.RobotClearance, num_positions: int) -> None

Creates an empty clearance with size() == 0 and num_positions as given.

2. __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: drake::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[drake::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. If in_environment_collision and/or in_self_collision is given, the return value will reflect those given value(s). This function subsumes the C++ free functions SetInEnvironmentCollision() and SetInSelfCollision().

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 time_advancement_strategy “Choice of Time Advancement Strategy” for further details. The default here matches the default value from multibody::MultibodyPlantConfig.

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 time_advancement_strategy “Choice of Time Advancement Strategy” for further details. The default here matches the default value from multibody::MultibodyPlantConfig.

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 time_advancement_strategy “Choice of Time Advancement Strategy” for further details. The default here matches the default value from multibody::MultibodyPlantConfig.

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.

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

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 state x using the linear policy.

Parameter time:

Current time.

Parameter x:

Current state.

Returns

Optimal CoMdd.

Precondition:

Plan() has already been called.

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.

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

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

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

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

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

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

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

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

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

2. 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()

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