# pydrake.systems.trajectory_optimization¶

pydrake.systems.trajectory_optimization.AddDirectCollocationConstraint(constraint: pydrake.systems.trajectory_optimization.DirectCollocationConstraint, timestep: 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.MathematicalProgram) → pydrake.solvers.mathematicalprogram.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.systems.trajectory_optimization.DirectCollocation

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.systems.trajectory_optimization.DirectCollocation, system: pydrake.systems.framework.System_[float], context: pydrake.systems.framework.Context_[float], num_time_samples: int, minimum_timestep: float, maximum_timestep: float, input_port_index: Union[pydrake.systems.framework.InputPortSelection, pydrake.systems.framework.InputPortIndex] = <InputPortSelection.kUseFirstInputIfItExists: -2>, assume_non_continuous_states_are_fixed: bool = False) → 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_timestep:
Minimum spacing between sample times.
Parameter maximum_timestep:
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.

class pydrake.systems.trajectory_optimization.DirectCollocationConstraint

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.systems.trajectory_optimization.DirectCollocationConstraint, system: pydrake.systems.framework.System_[float], context: pydrake.systems.framework.Context_[float], input_port_index: Union[pydrake.systems.framework.InputPortSelection, pydrake.systems.framework.InputPortIndex] = <InputPortSelection.kUseFirstInputIfItExists: -2>, assume_non_continuous_states_are_fixed: bool = False) → None

DirectCollocation constructor for a description of the parameters.

class pydrake.systems.trajectory_optimization.DirectTranscription

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)

1. __init__(self: pydrake.systems.trajectory_optimization.DirectTranscription, system: pydrake.systems.framework.System_[float], context: pydrake.systems.framework.Context_[float], 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 timestep update). Continuous-time systems must call one of the constructors that takes bounds on the timestep 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.

1. __init__(self: pydrake.systems.trajectory_optimization.DirectTranscription, system: pydrake.systems.framework.System_[float], context: pydrake.systems.framework.Context_[float], num_time_samples: int, fixed_timestep: pydrake.systems.trajectory_optimization.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_timestep:
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.

class pydrake.systems.trajectory_optimization.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.

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 timesteps h (N-1 of these), state x (N of these), and control input u (N of these).

__init__

Initialize self. See help(type(self)) for accurate signature.

AddCompleteTrajectoryCallback(self: pydrake.systems.trajectory_optimization.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.mathematicalprogram.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(self: pydrake.systems.trajectory_optimization.MultipleShooting, f: pydrake.symbolic.Formula) → List[pydrake.solvers.mathematicalprogram.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 constraints added to each knot point.
AddDurationBounds(self: pydrake.systems.trajectory_optimization.MultipleShooting, lower_bound: float, upper_bound: float) → pydrake.solvers.mathematicalprogram.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. RuntimeError if timesteps are not declared as decision variables.
AddEqualTimeIntervalsConstraints(self: pydrake.systems.trajectory_optimization.MultipleShooting) → List[pydrake.solvers.mathematicalprogram.Binding_LinearConstraint]

Adds constraints to enforce that all timesteps have equal duration.

Returns: A vector of constraints enforcing all time intervals are equal. RuntimeError if timesteps are not declared as decision variables.
AddFinalCost(*args, **kwargs)

1. AddFinalCost(self: pydrake.systems.trajectory_optimization.MultipleShooting, e: pydrake.symbolic.Expression) -> pydrake.solvers.mathematicalprogram.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.
1. AddFinalCost(self: pydrake.systems.trajectory_optimization.MultipleShooting, matrix: numpy.ndarray[object[m, n], flags.f_contiguous]) -> pydrake.solvers.mathematicalprogram.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.systems.trajectory_optimization.MultipleShooting, callback: Callable[[numpy.ndarray[numpy.float64[m, 1]], numpy.ndarray[numpy.float64[m, n], flags.f_contiguous]], None]) → pydrake.solvers.mathematicalprogram.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)

1. AddRunningCost(self: pydrake.systems.trajectory_optimization.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.

1. AddRunningCost(self: pydrake.systems.trajectory_optimization.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.systems.trajectory_optimization.MultipleShooting, callback: Callable[[numpy.ndarray[numpy.float64[m, 1]], numpy.ndarray[numpy.float64[m, n], flags.f_contiguous]], None]) → pydrake.solvers.mathematicalprogram.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.systems.trajectory_optimization.MultipleShooting, lower_bound: float, upper_bound: float) → pydrake.solvers.mathematicalprogram.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. RuntimeError if timesteps are not declared as decision variables.
final_state(self: pydrake.systems.trajectory_optimization.MultipleShooting) → numpy.ndarray[object[m, 1]]

Returns the decision variables associated with the state, x, at the final time index.

fixed_timestep(self: pydrake.systems.trajectory_optimization.MultipleShooting) → float
GetInputSamples(self: pydrake.systems.trajectory_optimization.MultipleShooting, result: pydrake.solvers.mathematicalprogram.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.systems.trajectory_optimization.MultipleShooting, result: pydrake.solvers.mathematicalprogram.MathematicalProgramResult) → numpy.ndarray[numpy.float64[m, 1]]

Returns a vector containing the elapsed time at each breakpoint.

GetSequentialVariableAtIndex(self: pydrake.systems.trajectory_optimization.MultipleShooting, name: str, index: int) → numpy.ndarray[object[m, 1]]

Returns the decision variables associated with the sequential variable name at time index index.

NewSequentialVariable().

GetSequentialVariableSamples(self: pydrake.systems.trajectory_optimization.MultipleShooting, result: pydrake.solvers.mathematicalprogram.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.systems.trajectory_optimization.MultipleShooting, result: pydrake.solvers.mathematicalprogram.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.systems.trajectory_optimization.MultipleShooting) → numpy.ndarray[object[m, 1]]

Returns the decision variables associated with the state, x, at the initial time index.

input(*args, **kwargs)

1. input(self: pydrake.systems.trajectory_optimization.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.

1. input(self: pydrake.systems.trajectory_optimization.MultipleShooting, index: int) -> numpy.ndarray[object[m, 1]]

Returns the decision variables associated with the input, u, at time index index.

NewSequentialVariable(self: pydrake.systems.trajectory_optimization.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.

ReconstructInputTrajectory(self: pydrake.systems.trajectory_optimization.MultipleShooting, result: pydrake.solvers.mathematicalprogram.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.systems.trajectory_optimization.MultipleShooting, result: pydrake.solvers.mathematicalprogram.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.systems.trajectory_optimization.MultipleShooting, traj_init_u: pydrake.trajectories.PiecewisePolynomial, traj_init_x: pydrake.trajectories.PiecewisePolynomial) → None
state(*args, **kwargs)

1. state(self: pydrake.systems.trajectory_optimization.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.

1. state(self: pydrake.systems.trajectory_optimization.MultipleShooting, index: int) -> numpy.ndarray[object[m, 1]]

Returns the decision variables associated with the state, x, at time index index.

time(self: pydrake.systems.trajectory_optimization.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.

timestep(self: pydrake.systems.trajectory_optimization.MultipleShooting, index: int) → numpy.ndarray[object[1, 1]]

Returns the decision variable associated with the timestep, h, at time index index.

Raises: RuntimeError if timesteps are not declared as decision variables.
class pydrake.systems.trajectory_optimization.TimeStep
__init__(self: pydrake.systems.trajectory_optimization.TimeStep, value: float) → None
value