pydrake.multibody.optimization

Optimization module for MultibodyPlant motion planning

class pydrake.multibody.optimization.CalcGridPointsOptions

__init__(self: pydrake.multibody.optimization.CalcGridPointsOptions, **kwargs) → None

property max_err

property max_iter

property max_seg_length

property min_points

class pydrake.multibody.optimization.CentroidalMomentumConstraint

Bases: pydrake.solvers.Constraint

Impose the constraint CentroidalMomentum(q, v) - h_WC = 0 with decision variables [q;v;h_WC] or CentroidalAngularMomentum(q, v) - k_WC = 0 with decision variables [q; v; k_WC] h_WC is the 6D spatial momentum (linear and angular momentum about the center of mass C) expressed in the world frame (W). k_WC is the 3D vector representing the angular momentum about the center of mass C expressed in the world frame W.

__init__(self: pydrake.multibody.optimization.CentroidalMomentumConstraint, plant: pydrake.multibody.plant.MultibodyPlant_[AutoDiffXd], model_instances: Optional[List[pydrake.multibody.tree.ModelInstanceIndex]], plant_context: pydrake.systems.framework.Context_[AutoDiffXd], angular_only: bool) → None

Construct centroidal momentum constraint If angular_only = false, we impose the constraint CentroidalMomentum(q, v) - h_WC = 0 where CentroidalMomentum computes the spatial momentum of the robot about its center-of-mass, expressed in the world frame. The decision variables are [q;v;h_WC]. If angular_only = true, we impose the constraint CentroidalAngularMomentum(q, v) - k_WC = 0 where CentroidalAngularMomentum(q, v) computes the angular momentum of the robot about its center-of-mass, expressed in the world frame. The decision variables are [q; v; k_WC]

Note

Currently, we can only construct this constraint using MultibodyPlant<AutoDiffXd> instead of MultibodyPlant<double>, since we can’t compute the Jacobian of the momentum using MultibodyPlant<double> yet.

Parameter plant:: The plant for which the constraint is imposed.
Parameter model_instances:: We compute the model with these model instances in plant. If model_instances=std::nullopt, then we compute the momentum of all model instances except the world.

class pydrake.multibody.optimization.ContactWrench

Stores the contact wrench (spatial force) from Body A to Body B applied at point Cb.

__init__(*args, **kwargs)

property bodyA_index: The index of Body A.

property bodyB_index: The index of Body B.

property F_Cb_W: F_Cb_W_in The wrench (spatial force) applied at point Cb from Body A to Body B, measured in the world frame.

property p_WCb_W: The position of the point Cb (where the wrench is applied) expressed in the world frame W.

class pydrake.multibody.optimization.ContactWrenchFromForceInWorldFrameEvaluator

Bases: pydrake.solvers.EvaluatorBase

The contact wrench is τ_AB_W = 0, f_AB_W = λ Namely we assume that λ is the contact force from A to B, applied directly at B’s witness point.

__init__(self: pydrake.multibody.optimization.ContactWrenchFromForceInWorldFrameEvaluator, plant: pydrake.multibody.plant.MultibodyPlant_[AutoDiffXd], context: pydrake.systems.framework.Context_[AutoDiffXd], geometry_id_pair: Tuple[pydrake.geometry.GeometryId]) → None

Parameter plant:: The MultibodyPlant on which the contact wrench is computed. The lifetime of plant should outlive this object.
Parameter context:: The context of the MultibodyPlant. The lifetime of context should outlive this object.
Parameter geometry_id_pair:: The pair of geometries for which the contact wrench is computed. Notice that the order of the geometries in the pair should match with that in SceneGraphInspector::GetCollisionCandidates().
Parameter description:: The description of this evaluator. Default to none.

class pydrake.multibody.optimization.QuaternionEulerIntegrationConstraint

Bases: pydrake.solvers.Constraint

If we have a body with orientation quaternion z₁ at time t₁, and a quaternion z₂ at time t₂ = t₁ + h, with the angular velocity ω (expressed in the world frame), we impose the constraint that the body rotates at a constant velocity ω from quaternion z₁ to quaternion z₂ within time interval h. Namely we want to enforce the relationship that z₂ and Δz⊗z₁ represent the same orientation, where Δz is the quaternion [cos(|ω|h/2), ω/|ω|*sin(|ω|h/2)], and ⊗ is the Hamiltonian product between quaternions.

It is well-known that for any quaternion z, its element-wise negation -z correspond to the same rotation matrix as z does. One way to understand this is that -z represents the rotation that first rotate the frame by a quaternion z, and then continue to rotate about that axis for 360 degrees. We provide the option allow_quaternion_negation flag, that if set to true, then we require that the quaternion z₂ = ±Δz⊗z₁. Otherwise we require z₂ = Δz⊗z₁. Mathematically, the constraint we impose is

If allow_quaternion_negation = true:

(z₂ • (Δz⊗z₁))² = 1

else

z₂ • (Δz⊗z₁) = 1

If your robot link orientation only changes slightly, and you are free to search for both z₁ and z₂, then we would recommend to set allow_quaternion_negation to false, as the left hand side of constraint z₂ • (Δz⊗z₁) = 1 is less nonlinear than the left hand side of (z₂ • (Δz⊗z₁))² = 1.

The operation • is the dot product between two quaternions, which computes the cosine of the half angle between these two orientations. Dot product equals to ±1 means that angle between the two quaternions are 2kπ, hence they represent the same orientation.

Note

The constraint is not differentiable at ω=0 (due to the non-differentiability of |ω| at ω = 0). So it is better to initialize the angular velocity to a non-zero value in the optimization.

The decision variables of this constraint are [z₁, z₂, ω, h]

Note

We need to evaluate sin(|ω|h/2)/|ω|, when h is huge (larger than 1/machine_epsilon), and |ω| is tiny (less than machine epsilon), this evaluation is inaccurate. So don’t use this constraint if you have a huge h (which would be bad practice in trajectory optimization anyway).

__init__(self: pydrake.multibody.optimization.QuaternionEulerIntegrationConstraint, allow_quaternion_negation: bool) → None

Parameter allow_quaternion_negation:: . Refer to the class documentation.

allow_quaternion_negation(self: pydrake.multibody.optimization.QuaternionEulerIntegrationConstraint) → bool

ComposeVariable(*args, **kwargs)

Overloaded function.

ComposeVariable(self: pydrake.multibody.optimization.QuaternionEulerIntegrationConstraint, quat1: numpy.ndarray[numpy.float64[4, 1]], quat2: numpy.ndarray[numpy.float64[4, 1]], angular_vel: numpy.ndarray[numpy.float64[3, 1]], h: float) -> numpy.ndarray[numpy.float64[12, 1]]
ComposeVariable(self: pydrake.multibody.optimization.QuaternionEulerIntegrationConstraint, quat1: numpy.ndarray[object[4, 1]], quat2: numpy.ndarray[object[4, 1]], angular_vel: numpy.ndarray[object[3, 1]], h: pydrake.symbolic.Variable) -> numpy.ndarray[object[12, 1]]
ComposeVariable(self: pydrake.multibody.optimization.QuaternionEulerIntegrationConstraint, quat1: numpy.ndarray[object[4, 1]], quat2: numpy.ndarray[object[4, 1]], angular_vel: numpy.ndarray[object[3, 1]], h: pydrake.symbolic.Expression) -> numpy.ndarray[object[12, 1]]

class pydrake.multibody.optimization.SpatialVelocityConstraint

Bases: pydrake.solvers.Constraint

Constrains the spatial velocity of a frame C, rigidly attached to a frame B, measured and expressed in frame A. It should be bound with decision variables corresponding to the multibody state x=[q,v] of the plant passed to the constructor.

__init__(self: pydrake.multibody.optimization.SpatialVelocityConstraint, plant: pydrake.multibody.plant.MultibodyPlant_[AutoDiffXd], frameA: pydrake.multibody.tree.Frame_[AutoDiffXd], v_AC_lower: numpy.ndarray[numpy.float64[3, 1]], v_AC_upper: numpy.ndarray[numpy.float64[3, 1]], frameB: pydrake.multibody.tree.Frame_[AutoDiffXd], p_BCo: numpy.ndarray[numpy.float64[3, 1]], plant_context: pydrake.systems.framework.Context_[AutoDiffXd], w_AC_bounds: Optional[drake::multibody::SpatialVelocityConstraint::AngularVelocityBounds] = None) → None

Constructs SpatialVelocityConstraint object.

Parameter plant:: The MultibodyPlant on which the constraint is imposed. plant should be alive during the lifetime of this constraint.
Parameter frameA:: The frame in which frame C’s spatial velocity is measured and expressed.
Parameter v_AC_lower:: The lower bound on the translational velocity of C, expressed in frame A.
Parameter v_AC_upper:: The upper bound on the translational velocity of C, expressed in frame A.
Parameter frameB:: The frame to which frame C is rigidly attached.
Parameter p_BCo:: The position of the origin of frame C, rigidly attached to frame B, expressed in frame B. We take R_BC to be the identity rotation.
Parameter plant_context:: The Context that has been allocated for this plant. We will update the context when evaluating the constraint. plant_context should be alive during the lifetime of this constraint.
Parameter w_AC_bounds:: If provided, then the number of constraints will be 5: 3 for translational velocities and then two more for the angular velocity magnitude and angle.
Precondition:: frameA and frameB must belong to plant.
Precondition:: v_AC_lower(i) <= v_AC_upper(i) for i = 1, 2, 3.

Raises

RuntimeError if plant is nullptr. –
RuntimeError if plant_context is nullptr. –
RuntimeError if invalid w_AC_bounds are provided. –

class AngularVelocityBounds

Parametrizes bounds on the magnitude and direction of the angular velocity vector.

__init__(self: pydrake.multibody.optimization.SpatialVelocityConstraint.AngularVelocityBounds) → None

Constructs SpatialVelocityConstraint object.

Parameter plant:: The MultibodyPlant on which the constraint is imposed. plant should be alive during the lifetime of this constraint.
Parameter frameA:: The frame in which frame C’s spatial velocity is measured and expressed.
Parameter v_AC_lower:: The lower bound on the translational velocity of C, expressed in frame A.
Parameter v_AC_upper:: The upper bound on the translational velocity of C, expressed in frame A.
Parameter frameB:: The frame to which frame C is rigidly attached.
Parameter p_BCo:: The position of the origin of frame C, rigidly attached to frame B, expressed in frame B. We take R_BC to be the identity rotation.
Parameter plant_context:: The Context that has been allocated for this plant. We will update the context when evaluating the constraint. plant_context should be alive during the lifetime of this constraint.
Parameter w_AC_bounds:: If provided, then the number of constraints will be 5: 3 for translational velocities and then two more for the angular velocity magnitude and angle.
Precondition:: frameA and frameB must belong to plant.
Precondition:: v_AC_lower(i) <= v_AC_upper(i) for i = 1, 2, 3.

Raises

RuntimeError if plant is nullptr. –
RuntimeError if plant_context is nullptr. –
RuntimeError if invalid w_AC_bounds are provided. –

property magnitude_lower: Lower bound on the magnitude of the angular velocity vector. Must be non-negative. The actual constraint will be implemented as a constraint on the squared magnitude.

property magnitude_upper: Upper bound on the magnitude of the angular velocity vector. Must be ≥ magnitude_lower. The actual constraint will be implemented as a constraint on the squared magnitude.

property reference_direction: Reference direction of the angular velocity vector. (Only the direction matters, the magnitude does not).

property theta_bound: The angle difference between w_AC and reference_direction will be constrained to θ ∈ [0,π] ≤ θ_bound. Must be nonnegative. The actual constraint will be implemented as cos(θ_bound) ≤ cos(θ) ≤ 1. When the norm of w_AC is very close to zero, we add a small number to the norm to avoid numerical difficulties.

class pydrake.multibody.optimization.StaticEquilibriumProblem

Finds the static equilibrium pose of a multibody system through optimization. The constraints are 1. 0 = g(q) + Bu + ∑ᵢ JᵢᵀFᵢ_AB_W(λᵢ) (generalized force equals to 0). 2. Fᵢ_AB_W(λᵢ) is within the admissible contact wrench (for example, contact force is in the friction cone). 3. sdf(q) >= 0 (signed distance function is no smaller than 0, hence no penetration). 4. complementarity condition between the contact force and the signed distance. 5. q within the joint limit.

__init__(self: pydrake.multibody.optimization.StaticEquilibriumProblem, plant: pydrake.multibody.plant.MultibodyPlant_[AutoDiffXd], context: pydrake.systems.framework.Context_[AutoDiffXd], ignored_collision_pairs: Set[Tuple[pydrake.geometry.GeometryId, pydrake.geometry.GeometryId]]) → None

Parameter plant:: The plant for which the static equilibrium posture is computed. plant should remain alive as long as this StaticEquilibriumProblem exists.
Parameter context:: The context for plant. context should remain alive as long as this StaticEquilibriumProblem exists.
Parameter ignored_collision_pairs:: The contact between the pair of geometry in ignored_collision_pairs will be ignored. We will not impose non-penetration constraint between these pairs, and no contact wrench will be applied between these pairs.

get_mutable_prog(self: pydrake.multibody.optimization.StaticEquilibriumProblem) → pydrake.solvers.MathematicalProgram

GetContactWrenchSolution(self: pydrake.multibody.optimization.StaticEquilibriumProblem, result: pydrake.solvers.MathematicalProgramResult) → List[pydrake.multibody.optimization.ContactWrench]

Retrieve the solution to all contact wrenches.

Parameter result:: The result of solving prog().

prog(self: pydrake.multibody.optimization.StaticEquilibriumProblem) → pydrake.solvers.MathematicalProgram: Getter for the immutable optimization program.

q_vars(self: pydrake.multibody.optimization.StaticEquilibriumProblem) → numpy.ndarray[object[m, 1]]: Getter for q, the decision variable for the generalized position.

u_vars(self: pydrake.multibody.optimization.StaticEquilibriumProblem) → numpy.ndarray[object[m, 1]]: Getter for u, the decision variable for the input.

UpdateComplementarityTolerance(self: pydrake.multibody.optimization.StaticEquilibriumProblem, tol: float) → None: Updates the tolerance on all the complementarity constraints α * β = 0. The complementarity constraint is relaxed as 0 ≤ α * β ≤ tol. See AddStaticFrictionConeComplementarityConstraint() for more details.

class pydrake.multibody.optimization.Toppra

Solves a Time Optimal Path Parameterization based on Reachability Analysis (TOPPRA) to find the fastest traversal of a given path, satisfying the given constraints. The approach is described in “A new approach to Time-Optimal Path Parameterization based on Reachability Analysis” by Hung Pham and Quang Cuong Pham, IEEE Transactions on Robotics, 2018.

__init__(self: pydrake.multibody.optimization.Toppra, path: pydrake.trajectories.Trajectory, plant: pydrake.multibody.plant.MultibodyPlant, gridpoints: numpy.ndarray[numpy.float64[m, 1]]) → None

Constructs an inverse kinematics problem for a MultibodyPlant. This constructor will create and own a context for

Parameter plant:: .
Parameter path:: The trajectory on which the TOPPRA problem will be solved.
Parameter plant:: The robot that will follow the solved trajectory. Used for enforcing torque and frame specific constraints.
Parameter gridpoints:: The points along the path to discretize the problem and enforce constraints at. The first and last gridpoint must equal the path start and end time respectively. Gridpoints must also be monotonically increasing.

Note

Toppra does not currently support plants that contain bodies with quaternion degrees of freedom. In addition, any plant where q̇ ≠ v will have undefined behavior.

Note

The path velocity, ṡ(t), is limited to be between 0 and 1e8 to ensure the reachable set calculated in the backward pass is always bounded.

AddFrameAccelerationLimit(*args, **kwargs)

Overloaded function.

AddFrameAccelerationLimit(self: pydrake.multibody.optimization.Toppra, constraint_frame: pydrake.multibody.tree.Frame, lower_limit: numpy.ndarray[numpy.float64[6, 1]], upper_limit: numpy.ndarray[numpy.float64[6, 1]], discretization: pydrake.multibody.optimization.ToppraDiscretization = <ToppraDiscretization.kInterpolation: 1>) -> Tuple[pydrake.solvers.Binding[LinearConstraint], pydrake.solvers.Binding[LinearConstraint]]

Adds a limit on the elements of the spatial acceleration of the given frame, measured and expressed in the world frame. The limits should be given as [α_WF, a_WF], where α_WF is the frame’s angular acceleration and a_WF is the frame’s translational acceleration.

Parameter constraint_frame:: The frame to limit the acceleration of.
Parameter lower_limit:: The lower acceleration limit for constraint_frame.
Parameter upper_limit:: The upper acceleration limit for constraint_frame.
Parameter discretization:: The discretization scheme to use for this linear constraint. See ToppraDiscretization for details.

Returns: A pair containing the linear constraints that will enforce the frame acceleration limit on the backward pass and forward pass respectively.

AddFrameAccelerationLimit(self: pydrake.multibody.optimization.Toppra, constraint_frame: pydrake.multibody.tree.Frame, lower_limit: pydrake.trajectories.Trajectory, upper_limit: pydrake.trajectories.Trajectory, discretization: pydrake.multibody.optimization.ToppraDiscretization = <ToppraDiscretization.kInterpolation: 1>) -> Tuple[pydrake.solvers.Binding[LinearConstraint], pydrake.solvers.Binding[LinearConstraint]]

A version of acceleration limit that uses a trajectory for the upper and lower limits.

Parameter constraint_frame:: The frame to limit the acceleration of.
Parameter lower_limit:: The lower acceleration limit trajectory for constraint_frame.
Parameter upper_limit:: The upper acceleration limit trajectory for constraint_frame.
Parameter discretization:: The discretization scheme to use for this linear constraint. See ToppraDiscretization for details.

Returns: A pair containing the linear constraints that will enforce the frame acceleration limit on the backward pass and forward pass respectively.

Precondition:: Both lower_limit and upper_limit trajectories must have values in ℜ⁶. The six-dimensional column vector is interpreted as [α_WF, a_WF], where α_WF is the frame’s angular acceleration and a_WF is the frame’s translational acceleration.

Raises

If the intervals [upper_limit.start_time(), –
upper_limit.end_time()] and [lower_limit.start_time(), –
lower_limit.end_time()] don't overlap with [path.start_time(), –
path.end_time()] –

Note

The constraints are only added in the constraint trajectories domains (where they overlap the path). The rest of the path is not constrained.

AddFrameTranslationalSpeedLimit(*args, **kwargs)

Overloaded function.

AddFrameTranslationalSpeedLimit(self: pydrake.multibody.optimization.Toppra, constraint_frame: pydrake.multibody.tree.Frame, upper_limit: float) -> pydrake.solvers.Binding[BoundingBoxConstraint]

Adds a limit on the magnitude of the translational velocity of the given frame, measured and expressed in the world frame.

Parameter constraint_frame:: The frame to limit the translational speed of.
Parameter upper_limit:: The upper translational speed limit for constraint_frame.

Returns: The bounding box constraint that will enforce the frame translational speed limit during the backward pass.

AddFrameTranslationalSpeedLimit(self: pydrake.multibody.optimization.Toppra, constraint_frame: pydrake.multibody.tree.Frame, upper_limit: pydrake.trajectories.Trajectory) -> pydrake.solvers.Binding[BoundingBoxConstraint]

A version of the frame translational speed limit that uses a trajectory for the limit.

Parameter constraint_frame:: The frame to limit the translational speed of.
Parameter upper_limit:: The upper translational speed limit trajectory for constraint_frame.
Precondition:: upper_limit must have scalar values (a 1x1 matrix).

Returns

The bounding box constraint that will enforce the frame translational speed limit during the backward pass.

Raises

If the interval [upper_limit.start_time(), upper_limit.end_time()] –
doesn't overlap with [path.start_time(), path.end_time()] –

Note

The constraints are only added between upper_limit.start_time() and upper_limit.end_time(). The rest of the path is not constrained.

AddFrameVelocityLimit(self: pydrake.multibody.optimization.Toppra, constraint_frame: pydrake.multibody.tree.Frame, lower_limit: numpy.ndarray[numpy.float64[6, 1]], upper_limit: numpy.ndarray[numpy.float64[6, 1]]) → pydrake.solvers.Binding[BoundingBoxConstraint]

Adds a limit on the elements of the spatial velocity of the given frame, measured and and expressed in the world frame. The limits should be given as [ω_WF, v_WF], where ω_WF is the frame’s angular velocity and v_WF is the frame’s translational velocity.

Parameter constraint_frame:: The frame to limit the velocity of.
Parameter lower_limit:: The lower velocity limit for constraint_frame.
Parameter upper_limit:: The upper velocity limit for constraint_frame.

Returns: The bounding box constraint that will enforce the frame velocity limit during the backward pass.

AddJointAccelerationLimit(self: pydrake.multibody.optimization.Toppra, lower_limit: numpy.ndarray[numpy.float64[m, 1]], upper_limit: numpy.ndarray[numpy.float64[m, 1]], discretization: pydrake.multibody.optimization.ToppraDiscretization = <ToppraDiscretization.kInterpolation: 1>) → Tuple[pydrake.solvers.Binding[LinearConstraint], pydrake.solvers.Binding[LinearConstraint]]

Adds an acceleration limit to all the degrees of freedom in the plant. The limits must be arranged in the same order as the entries in the path.

Parameter lower_limit:: The lower acceleration limit for each degree of freedom.
Parameter upper_limit:: The upper acceleration limit for each degree of freedom.
Parameter discretization:: The discretization scheme to use for this linear constraint. See ToppraDiscretization for details.

Returns: A pair containing the linear constraints that will enforce the acceleration limit on the backward pass and forward pass respectively.

AddJointTorqueLimit(self: pydrake.multibody.optimization.Toppra, lower_limit: numpy.ndarray[numpy.float64[m, 1]], upper_limit: numpy.ndarray[numpy.float64[m, 1]], discretization: pydrake.multibody.optimization.ToppraDiscretization = <ToppraDiscretization.kInterpolation: 1>) → Tuple[pydrake.solvers.Binding[LinearConstraint], pydrake.solvers.Binding[LinearConstraint]]

Adds a torque limit to all the degrees of freedom in the plant. The limits must be arranged in the same order as the entries in the path. This constrains the generalized torques applied to the plant and does not reason about contact forces.

Parameter lower_limit:: The lower torque limit for each degree of freedom.
Parameter upper_limit:: The upper torque limit for each degree of freedom.
Parameter discretization:: The discretization scheme to use for this linear constraint. See ToppraDiscretization for details.

Returns: A pair containing the linear constraints that will enforce the torque limit on the backward pass and forward pass respectively.

AddJointVelocityLimit(self: pydrake.multibody.optimization.Toppra, lower_limit: numpy.ndarray[numpy.float64[m, 1]], upper_limit: numpy.ndarray[numpy.float64[m, 1]]) → pydrake.solvers.Binding[BoundingBoxConstraint]

Adds a velocity limit to all the degrees of freedom in the plant. The limits must be arranged in the same order as the entries in the path.

Parameter lower_limit:: The lower velocity limit for each degree of freedom.
Parameter upper_limit:: The upper velocity limit for each degree of freedom.

static CalcGridPoints(path: pydrake.trajectories.Trajectory, options: pydrake.multibody.optimization.CalcGridPointsOptions) → numpy.ndarray[numpy.float64[m, 1]]

Takes a path and generates a sequence of gridpoints selected to control the interpolation error of the optimization. The gridpoints are selected such that the distance between them is below max_seg_length, there are at least min_points number of gridpoints and the interpolation error, estimated with the equation

Click to expand C++ code...

errₑₛₜ = max(|q̈ Δₛ²|) / 2

where Δₛ is the distance between sequential gridpoints, is less than max_err. Gridpoints are selected by adding the midpoint between two gridpoints whenever the distance between them is too large or the estimated error is too high. This results in more points in parts of the path with higher curvature. All grid points will lie between path.start_time() and path.end_time().

SolvePathParameterization(self: pydrake.multibody.optimization.Toppra) → Optional[pydrake.trajectories.PiecewisePolynomial]: Solves the TOPPRA optimization and returns the time optimized path parameterization s(t). This can be used with the original path q(s) to generate a time parameterized trajectory. The path parameterization has the same start time as the original path’s starting break.

class pydrake.multibody.optimization.ToppraDiscretization

Selects how linear constraints are enforced for TOPPRA’s optimization. kCollocation - enforces constraints only at each gridpoint. kInterpolation - enforces constraints at each gridpoint and at the following gridpoint using forward integration. Yields higher accuracy at minor computational cost.

Members:

kCollocation :

kInterpolation :

__init__(self: pydrake.multibody.optimization.ToppraDiscretization, value: int) → None

kCollocation = <ToppraDiscretization.kCollocation: 0>

kInterpolation = <ToppraDiscretization.kInterpolation: 1>

property name

property value