pydrake.systems.controllers
- pydrake.systems.controllers.DiscreteTimeLinearQuadraticRegulator(A: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], B: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], Q: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], R: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous]) tuple[numpy.ndarray[numpy.float64[m, n]], numpy.ndarray[numpy.float64[m, n]]]
Computes the optimal feedback controller, u=-Kx, and the optimal cost-to-go J = x’Sx for the problem:
\[x[n+1] = Ax[n] + Bu[n]\]\[\min_u \sum_0^\infty x'Qx + u'Ru\]- Parameter
A
: The state-space dynamics matrix of size num_states x num_states.
- Parameter
B
: The state-space input matrix of size num_states x num_inputs.
- Parameter
Q
: A symmetric positive semi-definite cost matrix of size num_states x num_states.
- Parameter
R
: A symmetric positive definite cost matrix of size num_inputs x num_inputs.
- Returns
A structure that contains the optimal feedback gain K and the quadratic cost term S. The optimal feedback control is u = -Kx;
- Raises
RuntimeError if R is not positive definite. –
- Parameter
- class pydrake.systems.controllers.DynamicProgrammingOptions
Consolidates the many possible options to be passed to the dynamic programming algorithms.
- __init__(self: pydrake.systems.controllers.DynamicProgrammingOptions) None
- property assume_non_continuous_states_are_fixed
(Advanced) 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.
- property convergence_tol
Value iteration methods converge when the value function stops changing (typically evaluated with the l∞ norm). This value sets that threshold.
- property discount_factor
A value between (0,1] that discounts future rewards.
See also
FittedValueIteration.
- property input_port_index
For systems with multiple input ports, we must specify which input port is being used in the control design.
See also
systems::InputPortSelection.
- property periodic_boundary_conditions
- class PeriodicBoundaryCondition
For algorithms that rely on approximations of the state-dynamics (as in FittedValueIteration), this is a list of state dimensions for which the state space maximum value should be “wrapped around” to ensure that all values are in the range [low, high). The classic example is for angles that are wrapped around at 2π.
- __init__(self: pydrake.systems.controllers.DynamicProgrammingOptions.PeriodicBoundaryCondition, state_index: int, low: float, high: float) None
- property high
- property low
- property state_index
- property visualization_callback
If callable, this method is invoked during each major iteration of the dynamic programming algorithm, in order to facilitate e.g. graphical inspection/debugging of the results.
Note
The first call happens at iteration 1 (after the value iteration has run once), not zero.
- pydrake.systems.controllers.FiniteHorizonLinearQuadraticRegulator(system: pydrake.systems.framework.System, context: pydrake.systems.framework.Context, t0: float, tf: float, Q: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], R: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], options: pydrake.systems.controllers.FiniteHorizonLinearQuadraticRegulatorOptions = FiniteHorizonLinearQuadraticRegulatorOptions(Qf=None, N=None, input_port_index=InputPortSelection.kUseFirstInputIfItExists, use_square_root_method=False, simulator_config=SimulatorConfig(integration_scheme='runge_kutta3', max_step_size=0.1, accuracy=0.0001, use_error_control=True, target_realtime_rate=0.0, publish_every_time_step=False))) pydrake.systems.controllers.FiniteHorizonLinearQuadraticRegulatorResult
Solves the differential Riccati equation to compute the optimal controller and optimal cost-to-go for the finite-horizon linear quadratic regulator:
\[\min_u (x(t_f)-x_d(t_f))'Q_f(x(t_f)-x_d(t_f)) + \int_{t_0}^{t_f}\](x(t)-x_d(t))’Q(x(t)-x_d(t)) dt + int_{t_0}^{t_f} (u(t)-u_d(t))’R(u(t)-u_d(t)) dt + int_{t_0}^{t_f} 2(x(t)-x_d(t))’N(u(t)-u_d(t)) dt text{s.t. } dot{x} - dot{x}_0(t) = A(t)(x(t) - x_0(t)) + B(t)(u(t) - u_0(t)) + c(t)
where A(t), B(t), and c(t) are taken from the gradients of the continuous-time dynamics ẋ = f(t,x,u), as A(t) = dfdx(t, x0(t), u0(t)), B(t) = dfdu(t, x0(t), u0(t)), and c(t) = f(t, x0(t), u0(t)) - ẋ0(t). x0(t) and u0(t) can be specified in
options
, otherwise are taken to be constant trajectories with values given bycontext
.- Parameter
system
: a System<double> representing the plant.
- Parameter
context
: a Context<double> used to pass the default input, state, and parameters. Note: Use
options
to specify time-varying nominal state and/or input trajectories.- Parameter
t0
: is the initial time.
- Parameter
tf
: is the final time (with tf > t0).
- Parameter
Q
: is nxn positive semi-definite.
- Parameter
R
: is mxm positive definite.
- Parameter
options
: is the optional FiniteHorizonLinearQuadraticRegulatorOptions.
- Precondition:
system
must be a System<double> with (only) n continuous state variables and m inputs. It must be convertible to System<AutoDiffXd>.
Note
Support for difference-equation systems (
See also
System<T>::IsDifferenceEquationSystem()) by solving the differential Riccati equation and richer specification of the objective are anticipated (they are listed in the code as TODOs).
- Parameter
- class pydrake.systems.controllers.FiniteHorizonLinearQuadraticRegulatorOptions
A structure to facilitate passing the myriad of optional arguments to the FiniteHorizonLinearQuadraticRegulator algorithms.
- __init__(self: pydrake.systems.controllers.FiniteHorizonLinearQuadraticRegulatorOptions) None
- property input_port_index
For systems with multiple input ports, we must specify which input port is being used in the control design.
See also
systems::InputPortSelection.
- property N
A num_states x num_inputs matrix that describes the running cost 2(x-xd(t))’N(u-ud(t)). If unset, then N will be set to the zero matrix.
- property Qf
A num_states x num_states positive semi-definite matrix which specified the cost at the final time. If unset, then Qf will be set to the zero matrix.
- property simulator_config
For continuous-time dynamical systems, the Riccati equation is solved by the Simulator (running backwards in time). Use this parameter to configure the simulator (e.g. choose non-default integrator or integrator parameters).
- property u0
A nominal input trajectory. The system is linearized about this trajectory. u0 must be defined over the entire interval, [t0, tf]. If null, then u0 is taken to be a constant trajectory (whose value is specified by the context passed into the LQR method).
- property ud
A desired input trajectory. The objective is to regulate to this trajectory – the input component of the quadratic running cost is (u-ud(t))’*R*(u-ud(t)). If null, then ud(t) = u0(t).
- property use_square_root_method
Enables the “square-root” method solution to the Riccati equation. This is slightly more expensive and potentially less numerically accurate (errors are bounded on the square root), but is more numerically robust. When
True
, then you must also set a (positive definite and symmetric) Qf in this options struct.
- property x0
A nominal state trajectory. The system is linearized about this trajectory. x0 must be defined over the entire interval [t0, tf]. If null, then x0 is taken to be a constant trajectory (whose value is specified by the context passed into the LQR method).
- class pydrake.systems.controllers.FiniteHorizonLinearQuadraticRegulatorResult
A structure that contains the basic FiniteHorizonLinearQuadraticRegulator results. The finite-horizon cost-to-go is given by (x-x0(t))’*S(t)*(x-x0(t)) + 2*(x-x₀(t))’sₓ(t) + s₀(t) and the optimal controller is given by u-u0(t) = -K(t)*(x-x₀(t)) - k₀(t). Please don’t overlook the factor of 2 in front of the sₓ(t) term.
- __init__(*args, **kwargs)
- property K
This K is the K_x term in the derivation notes.
- Type
Note
- property k0
- property S
- property s0
- property sx
- property u0
- property x0
- pydrake.systems.controllers.FittedValueIteration(arg0: pydrake.systems.analysis.Simulator, arg1: Callable[[pydrake.systems.framework.Context], float], arg2: list[set[float]], arg3: list[set[float]], arg4: float, arg5: pydrake.systems.controllers.DynamicProgrammingOptions) tuple[pydrake.systems.primitives.BarycentricMeshSystem, numpy.ndarray[numpy.float64[1, n]]]
Implements Fitted Value Iteration on a (triangulated) Barycentric Mesh, which designs a state-feedback policy to minimize the infinite-horizon cost ∑ γⁿ g(x[n],u[n]), where γ is the discount factor in
options
.For background, and a description of this algorithm, see http://underactuated.csail.mit.edu/underactuated.html?chapter=dp . It currently requires that the system to be optimized has only continuous state and it is assumed to be time invariant. This code makes a discrete-time approximation (using
time_step)
for the value iteration update.- Parameter
simulator
: contains the reference to the System being optimized and to a Context for that system, which may contain non-default Parameters, etc. The
simulator
is run fortime_step
seconds from every point on the mesh in order to approximate the dynamics; all of the simulation parameters (integrator, etc) are relevant during that evaluation.- Parameter
cost_function
: is the continuous-time instantaneous cost. This implementation of the discrete-time formulation above uses the approximation g(x,u) = time_step*cost_function(x,u).
- Parameter
state_grid
: defines the mesh on the state space used to represent the cost-to-go function and the resulting policy.
- Parameter
input_grid
: defines the discrete action space used in the value iteration update.
- Parameter
time_step
: a time in seconds used for the discrete-time approximation.
- Parameter
options
: optional DynamicProgrammingOptions structure.
- Returns
a std::pair containing the resulting policy, implemented as a BarycentricMeshSystem, and the RowVectorXd J that defines the expected cost-to-go on a BarycentricMesh using
state_grid
. The policy has a single vector input (which is the continuous state of the system passed in throughsimulator)
and a single vector output (which is the input of the system passed in throughsimulator)
.
- Parameter
- class pydrake.systems.controllers.InverseDynamics
Bases:
pydrake.systems.framework.LeafSystem
Solves inverse dynamics with no consideration for joint actuator force limits.
Computes the generalized force
τ_id
that needs to be applied so that the multibody system undergoes a desired accelerationvd_d
. That is,τ_id
is the result of an inverse dynamics computation according to:Click to expand C++ code...
τ_id = M(q)vd_d + C(q, v)v - τ_g(q) - τ_app
where
M(q)
is the mass matrix,C(q, v)v
is the bias term containing Coriolis and gyroscopic effects,τ_g(q)
is the vector of generalized forces due to gravity andτ_app
contains applied forces from force elements added to the multibody model (this can include damping, springs, etc. See MultibodyPlant::CalcForceElementsContribution()).The system also provides a pure gravity compensation mode via an option in the constructor. In this case, the output is simply
Click to expand C++ code...
τ_id = -τ_g(q).
Note
As an alternative to adding a controller to your diagram, gravity compensation can be modeled by disabling gravity for a given model instance, see MultibodyPlant::set_gravity_enabled().
InverseDynamicsController uses a PID controller to generate desired acceleration and uses this class to compute generalized forces. Use this class directly if desired acceleration is computed differently.
estimated_state→ desired_acceleration→ InverseDynamics → generalized_force The desired acceleration port shown in <span style=”color:gray”>gray</span> is only present when the
mode
at construction is notkGravityCompensation
.- __init__(self: pydrake.systems.controllers.InverseDynamics, plant: pydrake.multibody.plant.MultibodyPlant, mode: pydrake.systems.controllers.InverseDynamics.InverseDynamicsMode = <InverseDynamicsMode.kInverseDynamics: 0>, plant_context: pydrake.systems.framework.Context = None) None
Constructs the InverseDynamics system.
- Parameter
plant
: Pointer to the multibody plant model. The life span of
plant
must be longer than that of this instance.- Parameter
mode
: If set to kGravityCompensation, this instance will only consider the gravity term. It also will NOT have the desired acceleration input port.
- Parameter
plant_context
: A specific context of
plant
to use for computing inverse dynamics. For example, you can use this to pass in a context with modified mass parameters. Ifnullptr
, the default context of the givenplant
is used. Note that this will be copied at time of construction, so there are no lifetime constraints.- Precondition:
The plant must be finalized (i.e., plant.is_finalized() must return
True
). Also,plant_context
, if provided, must be compatible withplant
.
- Parameter
- get_input_port_desired_acceleration(self: pydrake.systems.controllers.InverseDynamics) pydrake.systems.framework.InputPort
Returns the input port for the desired acceleration.
- get_input_port_estimated_state(self: pydrake.systems.controllers.InverseDynamics) pydrake.systems.framework.InputPort
Returns the input port for the estimated state.
- get_output_port_generalized_force(self: pydrake.systems.controllers.InverseDynamics) pydrake.systems.framework.OutputPort
Returns the output port for the generalized forces that realize the desired acceleration. The dimension of that force vector will be identical to the dimensionality of the generalized velocities.
- class InverseDynamicsMode
Members:
kInverseDynamics :
kGravityCompensation : Purely gravity compensation mode.
- __init__(self: pydrake.systems.controllers.InverseDynamics.InverseDynamicsMode, value: int) None
- kGravityCompensation = <InverseDynamicsMode.kGravityCompensation: 1>
- kInverseDynamics = <InverseDynamicsMode.kInverseDynamics: 0>
- property name
- property value
- is_pure_gravity_compensation(self: pydrake.systems.controllers.InverseDynamics) bool
- kGravityCompensation = <InverseDynamicsMode.kGravityCompensation: 1>
- kInverseDynamics = <InverseDynamicsMode.kInverseDynamics: 0>
- class pydrake.systems.controllers.InverseDynamicsController
Bases:
pydrake.systems.framework.Diagram
A state feedback controller that uses a PidController to generate desired accelerations, which are then converted into torques using InverseDynamics. More specifically, the output of this controller is:
Click to expand C++ code...
force = inverse_dynamics(q, v, vd_command), where vd_command = kp(q_d - q) + kd(v_d - v) + ki int(q_d - q) + vd_d.
Here
q
andv
stand for the generalized position and velocity, andvd
is the generalized acceleration. The subscript_d
indicates desired values, andvd_command
indicates the acceleration command (which includes the stabilization terms) passed to the inverse dynamics computation.estimated_state→ desired_state→ desired_acceleration→ InverseDynamicsController → generalized_force Note
As an alternative to adding a separate controller system to your diagram, you can model gravity compensation with PD controllers using MultibodyPlant APIs. Refer to MultibodyPlant::set_gravity_enabled() as an alternative to modeling gravity compensation. To model PD controlled actuators, refer to mbp_actuation “Actuation”.
The desired acceleration port shown in <span style=”color:gray”>gray</span> may be absent, depending on the arguments passed to the constructor.
Note that this class assumes the robot is fully actuated, its position and velocity have the same dimension, and it does not have a floating base. If violated, the program will abort. This controller was not designed for closed-loop systems: the controller accounts for neither constraint forces nor actuator forces applied at loop constraints. Use on such systems is not recommended.
See also
InverseDynamics for an accounting of all forces incorporated into the inverse dynamics computation.
- __init__(self: pydrake.systems.controllers.InverseDynamicsController, robot: pydrake.multibody.plant.MultibodyPlant, kp: numpy.ndarray[numpy.float64[m, 1]], ki: numpy.ndarray[numpy.float64[m, 1]], kd: numpy.ndarray[numpy.float64[m, 1]], has_reference_acceleration: bool, plant_context: pydrake.systems.framework.Context = None) None
Constructs an inverse dynamics controller for the given
plant
model. The InverseDynamicsController holds an internal, non-owned reference to the MultibodyPlant object so you must ensure thatplant
has a longer lifetime thanthis
InverseDynamicsController.- Parameter
plant
: The model of the plant for control.
- Parameter
kp
: Position gain.
- Parameter
ki
: Integral gain.
- Parameter
kd
: Velocity gain.
- Parameter
has_reference_acceleration
: If true, there is an extra BasicVector input port for
vd_d
. If false,vd_d
is treated as zero, and no extra input port is declared.- Parameter
plant_context
: The context of the
plant
that can be used to override the plant’s default parameters. Note that this will be copied at time of construction, so there are no lifetime constraints.- Precondition:
plant
has been finalized (plant.is_finalized() returnsTrue
). Also,plant
andplant_context
must be compatible.
- Raises
RuntimeError if - The plant is not finalized (see –
MultibodyPlant::Finalize()) - The plant is not compatible with –
the plant context. - The number of generalized velocities is not –
equal to the number of generalized positions. - The model is not –
fully actuated. - Vector kp, ki and kd do not all have the same –
size equal to the number of generalized positions. –
- Parameter
- get_input_port_desired_acceleration(self: pydrake.systems.controllers.InverseDynamicsController) pydrake.systems.framework.InputPort
Returns the input port for the reference acceleration.
- get_input_port_desired_state(self: pydrake.systems.controllers.InverseDynamicsController) pydrake.systems.framework.InputPort
Returns the input port for the desired state.
- get_input_port_estimated_state(self: pydrake.systems.controllers.InverseDynamicsController) pydrake.systems.framework.InputPort
Returns the input port for the estimated state.
- get_multibody_plant_for_control(self: pydrake.systems.controllers.InverseDynamicsController) pydrake.multibody.plant.MultibodyPlant
Returns a constant pointer to the MultibodyPlant used for control.
- get_output_port_control(self: pydrake.systems.controllers.InverseDynamicsController) pydrake.systems.framework.OutputPort
Returns the output port for computed control.
- set_integral_value(self: pydrake.systems.controllers.InverseDynamicsController, arg0: pydrake.systems.framework.Context, arg1: numpy.ndarray[numpy.float64[m, 1]]) None
Sets the integral part of the PidController to
value
.value
must be a column vector of the appropriate size.
- class pydrake.systems.controllers.JointStiffnessController
Bases:
pydrake.systems.framework.LeafSystem
Implements a joint-space stiffness controller of the form
Click to expand C++ code...
τ_control = −τ_g(q) − τ_app + kp⊙(q_d − q) + kd⊙(v_d − v)
where
Kp
andKd
are the joint stiffness and damping coefficients, respectively,τ_g(q)
is the vector of generalized forces due to gravity, andτ_app
contains applied forces from force elements added to the multibody model (this can include damping, springs, etc. See MultibodyPlant::CalcForceElementsContribution()).q_d
andv_d
are the desired (setpoint) values for the multibody positions and velocities, respectively.kd
andkp
are taken as vectors, and ⊙ represents elementwise multiplication.The goal of this controller is to produce a closed-loop dynamics that resembles a spring-damper dynamics at the joints around the setpoint:
Click to expand C++ code...
M(q)v̇ + C(q,v)v + kp⊙(q - q_d) + kd⊙(v - v_d) = τ_ext,
where
M(q)v̇ + C(q,v)v
are the original multibody mass and Coriolis terms, andτ_ext
are any external generalized forces that can arise, e.g. from contact forces.The controller currently requires that plant.num_positions() == plant.num_velocities() == plant.num_actuated_dofs() and that plant.IsVelocityEqualToQDot() is true.
estimated_state→ desired_state→ JointStiffnessController → generalized_force Note that the joint impedance control as implemented on Kuka’s iiwa and Franka’ Panda is best modeled as a stiffness controller (unless one were to model the actual elastic joints and rotor-inertia shaping). See https://manipulation.csail.mit.edu/force.html for more details.
- __init__(self: pydrake.systems.controllers.JointStiffnessController, plant: pydrake.multibody.plant.MultibodyPlant, kp: numpy.ndarray[numpy.float64[m, 1]], kd: numpy.ndarray[numpy.float64[m, 1]]) None
Constructs the JointStiffnessController system.
- Parameter
plant
: Reference to the multibody plant model. The life span of
plant
must be at least as long as that of this instance.- Precondition:
The plant must be finalized (i.e., plant.is_finalized() must return
True
).- Precondition:
plant.num_positions() == plant.num_velocities() == plant.num_actuated_dofs() == kp.size() == kd.size()
- Precondition:
plant.IsVelocityEqualToQDot() is true.
- Parameter
- get_input_port_desired_state(self: pydrake.systems.controllers.JointStiffnessController) pydrake.systems.framework.InputPort
Returns the input port for the desired state.
- get_input_port_estimated_state(self: pydrake.systems.controllers.JointStiffnessController) pydrake.systems.framework.InputPort
Returns the input port for the estimated state.
- get_multibody_plant(self: pydrake.systems.controllers.JointStiffnessController) pydrake.multibody.plant.MultibodyPlant
Returns a constant pointer to the MultibodyPlant used for control.
- get_output_port_generalized_force(self: pydrake.systems.controllers.JointStiffnessController) pydrake.systems.framework.OutputPort
Returns the output port for the generalized forces implementing the control.
- pydrake.systems.controllers.LinearProgrammingApproximateDynamicProgramming(arg0: pydrake.systems.analysis.Simulator, arg1: Callable[[pydrake.systems.framework.Context], float], arg2: Callable[[numpy.ndarray[numpy.float64[m, 1]], numpy.ndarray[object[m, 1]]], pydrake.symbolic.Expression], arg3: int, arg4: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], arg5: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], arg6: float, arg7: pydrake.systems.controllers.DynamicProgrammingOptions) numpy.ndarray[numpy.float64[m, 1]]
Implements the Linear Programming approach to approximate dynamic programming. It optimizes the linear program
maximize ∑ Jₚ(x). subject to ∀x, ∀u, Jₚ(x) ≤ g(x,u) + γJₚ(f(x,u)),
where g(x,u) is the one-step cost, Jₚ(x) is a (linearly) parameterized cost-to-go function with parameter vector p, and γ is the discount factor in
options
.For background, and a description of this algorithm, see http://underactuated.csail.mit.edu/underactuated.html?chapter=dp . It currently requires that the system to be optimized has only continuous state and it is assumed to be time invariant. This code makes a discrete-time approximation (using
time_step)
for the value iteration update.- Parameter
simulator
: contains the reference to the System being optimized and to a Context for that system, which may contain non-default Parameters, etc. The
simulator
is run fortime_step
seconds from every pair of input/state sample points in order to approximate the dynamics; all of the simulation parameters (integrator, etc) are relevant during that evaluation.- Parameter
cost_function
: is the continuous-time instantaneous cost. This implementation of the discrete-time formulation above uses the approximation g(x,u) = time_step*cost_function(x,u).
- Parameter
linearly_parameterized_cost_to_go_function
: must define a function to approximate the cost-to-go, which takes the state vector as the first input and the parameter vector as the second input. This can be any function of the form Jₚ(x) = ∑ pᵢ φᵢ(x). This algorithm will pass in a VectorX of symbolic::Variable in order to set up the linear program.
- Parameter
state_samples
: is a list of sample states (one per column) at which to apply the optimization constraints and the objective.
- Parameter
input_samples
: is a list of inputs (one per column) which are evaluated at every sample point.
- Parameter
time_step
: a time in seconds used for the discrete-time approximation.
- Parameter
options
: optional DynamicProgrammingOptions structure.
- Returns
params the VectorXd of parameters that optimizes the supplied cost-to-go function.
- Parameter
- pydrake.systems.controllers.LinearQuadraticRegulator(*args, **kwargs)
Overloaded function.
LinearQuadraticRegulator(A: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], B: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], Q: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], R: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], N: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous] = array([], shape=(0, 0), dtype=float64), F: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous] = array([], shape=(0, 0), dtype=float64)) -> tuple[numpy.ndarray[numpy.float64[m, n]], numpy.ndarray[numpy.float64[m, n]]]
Computes the optimal feedback controller, u=-Kx, and the optimal cost-to-go J = x’Sx for the problem:
\[\min_u \int_0^\infty x'Qx + u'Ru + 2x'Nu dt\]\[\dot{x} = Ax + Bu\]\[Fx = 0\]- Parameter
A
: The state-space dynamics matrix of size num_states x num_states.
- Parameter
B
: The state-space input matrix of size num_states x num_inputs.
- Parameter
Q
: A symmetric positive semi-definite cost matrix of size num_states x num_states.
- Parameter
R
: A symmetric positive definite cost matrix of size num_inputs x num_inputs.
- Parameter
N
: A cost matrix of size num_states x num_inputs. If N.rows() == 0, N will be treated as a num_states x num_inputs zero matrix.
- Parameter
F
: A constraint matrix of size num_constraints x num_states. rank(F) must be < num_states. If F.rows() == 0, F will be treated as a 0 x num_states zero matrix.
- Returns
A structure that contains the optimal feedback gain K and the quadratic cost term S. The optimal feedback control is u = -Kx;
- Raises
RuntimeError if R is not positive definite. –
Note
The system (A₁, B) should be stabilizable, where A₁=A−BR⁻¹Nᵀ.
Note
The system (Q₁, A₁) should be detectable, where Q₁=Q−NR⁻¹Nᵀ.
LinearQuadraticRegulator(system: pydrake.systems.primitives.LinearSystem, Q: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], R: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], N: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous] = array([], shape=(0, 0), dtype=float64)) -> pydrake.systems.primitives.LinearSystem
Creates a system that implements the optimal time-invariant linear quadratic regulator (LQR). If
system
is a continuous-time system, then solves the continuous-time LQR problem:\[\min_u \int_0^\infty x^T(t)Qx(t) + u^T(t)Ru(t) + + 2x^T(t)Nu(t) dt.\]If
system
is a discrete-time system, then solves the discrete-time LQR problem:\[\min_u \sum_0^\infty x^T[n]Qx[n] + u^T[n]Ru[n] + 2x^T[n]Nu[n].\]- Parameter
system
: The System to be controlled.
- Parameter
Q
: A symmetric positive semi-definite cost matrix of size num_states x num_states.
- Parameter
R
: A symmetric positive definite cost matrix of size num_inputs x num_inputs.
- Parameter
N
: A cost matrix of size num_states x num_inputs.
- Returns
A system implementing the optimal controller in the original system coordinates.
- Raises
RuntimeError if R is not positive definite. –
LinearQuadraticRegulator(system: pydrake.systems.framework.System, context: pydrake.systems.framework.Context, Q: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], R: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], N: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous] = array([], shape=(0, 0), dtype=float64), input_port_index: int = 0) -> pydrake.systems.primitives.AffineSystem
Linearizes the System around the specified Context, computes the optimal time-invariant linear quadratic regulator (LQR), and returns a System which implements that regulator in the original System’s coordinates. If
system
is a continuous-time system, then solves the continuous-time LQR problem:\[\min_u \int_0^\infty (x-x_0)^TQ(x-x_0) + (u-u_0)^TR(u-u_0) + 2 (x-x_0)^TN(u-u_0) dt.\]If
system
is a discrete-time system, then solves the discrete-time LQR problem:\[\min_u \sum_0^\infty (x-x_0)^TQ(x-x_0) + (u-u_0)^TR(u-u_0) + 2(x-x_0)^TN(u-u_0),\]where \(x_0\) is the nominal state and \(u_0\) is the nominal input. The system is considered discrete if it has a single discrete state vector and a single unique periodic update event declared.
- Parameter
system
: The System to be controlled.
- Parameter
context
: Defines the desired state and control input to regulate the system to. Note that this state/input must be an equilibrium point of the system. See drake::systems::Linearize for more details.
- Parameter
Q
: A symmetric positive semi-definite cost matrix of size num_states x num_states.
- Parameter
R
: A symmetric positive definite cost matrix of size num_inputs x num_inputs.
- Parameter
N
: A cost matrix of size num_states x num_inputs. If the matrix is zero-sized, N will be treated as a num_states x num_inputs zero matrix.
- Parameter
input_port_index
: The index of the input port to linearize around.
- Returns
A system implementing the optimal controller in the original system coordinates.
- Raises
RuntimeError if R is not positive definite. $See also –
drake::systems::Linearize()
- pydrake.systems.controllers.MakeFiniteHorizonLinearQuadraticRegulator(system: pydrake.systems.framework.System, context: pydrake.systems.framework.Context, t0: float, tf: float, Q: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], R: numpy.ndarray[numpy.float64[m, n], flags.f_contiguous], options: pydrake.systems.controllers.FiniteHorizonLinearQuadraticRegulatorOptions = FiniteHorizonLinearQuadraticRegulatorOptions(Qf=None, N=None, input_port_index=InputPortSelection.kUseFirstInputIfItExists, use_square_root_method=False, simulator_config=SimulatorConfig(integration_scheme='runge_kutta3', max_step_size=0.1, accuracy=0.0001, use_error_control=True, target_realtime_rate=0.0, publish_every_time_step=False))) pydrake.systems.framework.System
Variant of FiniteHorizonLinearQuadraticRegulator that returns a System implementing the regulator (controller) as a System, with a single “plant_state” input for the estimated plant state, and a single “control” output for the regulator control output.
See also
FiniteHorizonLinearQuadraticRegulator for details on the arguments.
- class pydrake.systems.controllers.PeriodicBoundaryCondition
For algorithms that rely on approximations of the state-dynamics (as in FittedValueIteration), this is a list of state dimensions for which the state space maximum value should be “wrapped around” to ensure that all values are in the range [low, high). The classic example is for angles that are wrapped around at 2π.
- __init__(self: pydrake.systems.controllers.DynamicProgrammingOptions.PeriodicBoundaryCondition, state_index: int, low: float, high: float) None
- property high
- property low
- property state_index
- class pydrake.systems.controllers.PidControlledSystem
Bases:
pydrake.systems.framework.Diagram
A system that encapsulates a PidController and a controlled System (a.k.a the “plant”).
The passed in plant must meet the following properties:
Input port
plant_input_port_index
must be all of the control inputs (size U). When the plant is a dynamics model, this is typically the generalized effort (e.g., force or torque) command.The output port passed to the PidControlledSystem constructor must be of size 2 * Q, where the first Q elements are the position states of the plant, and the second Q elements are the velocity states of the plant. Q >= U.
The resulting PidControlledSystem has two input ports with the following properties:
Input port zero is the feed forward control (size U), which will be added onto the output of the PID controller. The sum is sent to the plant’s input.
Input port one is the desired controlled states (2 * U) of the plant, where the first half are the controlled positions, and the second half are the controlled velocities.
All output ports of the plant are exposed as output ports of the PidControlledSystem in the same order (and therefore with the same index) as they appear in the plant.
Some of the constructors include a parameter called
feedback_selector
. It is used to select the controlled states from the plant’s state output port. LetS
be the gain matrix in parameterfeedback_selector
. S must have dimensions of(2 * U, 2 * Q)
. Typically,S
contains one1
in each row, and zeros everywhere else.S
does not affect the desired state input. Let ‘x’ be the full state of the plant (size 2 * Q), and ‘x_d’ be the desired state (size 2 * U),S
is used to compute the state error asx_err = S * x - x_d
.desired_state→ feedforward_control→ PidControlledSystem → (exported output port of plant, with same name) → ... → (exported output port of plant, with same name) - __init__(*args, **kwargs)
Overloaded function.
__init__(self: pydrake.systems.controllers.PidControlledSystem, plant: pydrake.systems.framework.System, kp: float, ki: float, kd: float, state_output_port_index: int = 0, plant_input_port_index: int = 0) -> None
plant
full state is used for feedback control, and all the dimensions have homogeneous gains specified byKp
,Kd
andKi
.- Parameter
plant
: The system to be controlled. This must not be
nullptr
.- Parameter
Kp
: the proportional constant.
- Parameter
Ki
: the integral constant.
- Parameter
Kd
: the derivative constant.
- Parameter
state_output_port_index
: identifies the output port on the plant that contains the (full) state information.
- Parameter
plant_input_port_index
: identifies the input port on the plant that takes in the input (computed as the output of the PID controller).
__init__(self: pydrake.systems.controllers.PidControlledSystem, plant: pydrake.systems.framework.System, kp: numpy.ndarray[numpy.float64[m, 1]], ki: numpy.ndarray[numpy.float64[m, 1]], kd: numpy.ndarray[numpy.float64[m, 1]], state_output_port_index: int = 0, plant_input_port_index: int = 0) -> None
plant
full state is used for feedback control, and the vectorized gains are specified byKp
,Kd
andKi
.- Parameter
plant
: The system to be controlled. This must not be
nullptr
.- Parameter
Kp
: the proportional vector constant.
- Parameter
Ki
: the integral vector constant.
- Parameter
Kd
: the derivative vector constant.
- Parameter
state_output_port_index
: identifies the output port on the plant that contains the (full) state information.
- Parameter
plant_input_port_index
: identifies the input port on the plant that takes in the input (computed as the output of the PID controller).
__init__(self: pydrake.systems.controllers.PidControlledSystem, plant: pydrake.systems.framework.System, feedback_selector: numpy.ndarray[numpy.float64[m, n]], kp: float, ki: float, kd: float, state_output_port_index: int = 0, plant_input_port_index: int = 0) -> None
A constructor where the gains are scalar values and some of the plant’s output is part of the feedback signal as specified by
feedback_selector
.- Parameter
plant
: The system to be controlled. This must not be
nullptr
.- Parameter
feedback_selector
: The matrix that selects which part of the plant’s full state is fed back to the PID controller. For semantic details of this parameter, see this class’s description.
- Parameter
Kp
: the proportional constant.
- Parameter
Ki
: the integral constant.
- Parameter
Kd
: the derivative constant.
- Parameter
state_output_port_index
: identifies the output port on the plant that contains the (full) state information.
- Parameter
plant_input_port_index
: identifies the input port on the plant that takes in the input (computed as the output of the PID controller).
__init__(self: pydrake.systems.controllers.PidControlledSystem, plant: pydrake.systems.framework.System, feedback_selector: numpy.ndarray[numpy.float64[m, n]], kp: numpy.ndarray[numpy.float64[m, 1]], ki: numpy.ndarray[numpy.float64[m, 1]], kd: numpy.ndarray[numpy.float64[m, 1]], state_output_port_index: int = 0, plant_input_port_index: int = 0) -> None
A constructor where the gains are vector values and some of the plant’s output is part of the feedback signal as specified by
feedback_selector
.- Parameter
plant
: The system to be controlled. This must not be
nullptr
.- Parameter
feedback_selector
: The matrix that selects which part of the plant’s full state is fed back to the PID controller. For semantic details of this parameter, see this class’s description.
- Parameter
Kp
: the proportional vector constant.
- Parameter
Ki
: the integral vector constant.
- Parameter
Kd
: the derivative vector constant.
- Parameter
state_output_port_index
: identifies the output port on the plant that contains the (full) state information.
- Parameter
plant_input_port_index
: identifies the input port on the plant that takes in the input (computed as the output of the PID controller).
- get_control_input_port(self: pydrake.systems.controllers.PidControlledSystem) pydrake.systems.framework.InputPort
- Returns
the input port for the feed forward control input.
- get_state_input_port(self: pydrake.systems.controllers.PidControlledSystem) pydrake.systems.framework.InputPort
- Returns
the input port for the desired position/velocity state.
- get_state_output_port(self: pydrake.systems.controllers.PidControlledSystem) pydrake.systems.framework.OutputPort
- class pydrake.systems.controllers.PidController
Bases:
pydrake.systems.framework.LeafSystem
Implements the PID controller. Given estimated state
x_in = (q_in, v_in)
, the controlled statex_c = (q_c, v_c)
is computed byx_c = P_x * x_in
, whereP_x
is a state projection matrix. The desired statex_d = (q_d, v_d)
, is in the same space asx_c
. The output of this controller is:Click to expand C++ code...
y = P_y * (kp * (q_d - q_c) + kd * (v_d - v_c) + ki * integral(q_d - q_c)),
where
P_y
is the output projection matrix.estimated_state→ desired_state→ PidController → control This system has one continuous state, which is the integral of position error, two input ports: estimated state
x_in
and desired statex_d
, and one output porty
. Note that this class assumes|q_c| = |v_c|
and|q_d| = |v_d|
. However,|q_c|
does not have to equal to|q_d|
. One typical use case for non-identityP_x
andP_y
is to select a subset of state for feedback.- __init__(*args, **kwargs)
Overloaded function.
__init__(self: pydrake.systems.controllers.PidController, kp: numpy.ndarray[numpy.float64[m, 1]], ki: numpy.ndarray[numpy.float64[m, 1]], kd: numpy.ndarray[numpy.float64[m, 1]]) -> None
Constructs a PID controller.
P_x
andP_y
are identity matrices of proper sizes. The estimated and desired state inputs are 2 *kp
’s size, and the control output haskp
’s size.- Parameter
kp
: P gain.
- Parameter
ki
: I gain.
- Parameter
kd
: D gain.
- Raises
RuntimeError if kp, ki and kd have different –
dimensions. –
__init__(self: pydrake.systems.controllers.PidController, state_projection: numpy.ndarray[numpy.float64[m, n]], kp: numpy.ndarray[numpy.float64[m, 1]], ki: numpy.ndarray[numpy.float64[m, 1]], kd: numpy.ndarray[numpy.float64[m, 1]]) -> None
Constructs a PID controller. Calls the full constructor, with the output projection matrix
P_y
being the identity matrix.- Parameter
state_projection
: The state projection matrix
P_x
.- Parameter
kp
: P gain.
- Parameter
ki
: I gain.
- Parameter
kd
: D gain.
- Raises
RuntimeError if kp, ki and kd have different –
dimensions or `P_x.row() != 2 * kp'. –
__init__(self: pydrake.systems.controllers.PidController, state_projection: numpy.ndarray[numpy.float64[m, n]], output_projection: numpy.ndarray[numpy.float64[m, n]], kp: numpy.ndarray[numpy.float64[m, 1]], ki: numpy.ndarray[numpy.float64[m, 1]], kd: numpy.ndarray[numpy.float64[m, 1]]) -> None
Constructs a PID controller. This assumes that
Click to expand C++ code...
1. |kp| = |kd| = |ki| = |q_d| = |v_d| 2. 2 * |q_d| = P_x.rows 3. |x_in| = P_x.cols 4. |y| = P_y.rows 4. |q_d| = P_y.cols
- Parameter
state_projection
: The state projection matrix
P_x
.- Parameter
output_projection
: The output projection matrix
P_y
.- Parameter
kp
: P gain.
- Parameter
ki
: I gain.
- Parameter
kd
: V gain.
- Raises
RuntimeError if any assumption is violated. –
- get_input_port_desired_state(self: pydrake.systems.controllers.PidController) pydrake.systems.framework.InputPort
Returns the input port for the desired state.
- get_input_port_estimated_state(self: pydrake.systems.controllers.PidController) pydrake.systems.framework.InputPort
Returns the input port for the estimated state.
- get_Kd_vector(self: pydrake.systems.controllers.PidController) numpy.ndarray[numpy.float64[m, 1]]
Returns the derivative gain vector.
- get_Ki_vector(self: pydrake.systems.controllers.PidController) numpy.ndarray[numpy.float64[m, 1]]
Returns the integral gain vector.
- get_Kp_vector(self: pydrake.systems.controllers.PidController) numpy.ndarray[numpy.float64[m, 1]]
Returns the proportional gain vector.
- get_output_port_control(self: pydrake.systems.controllers.PidController) pydrake.systems.framework.OutputPort
Returns the output port for computed control.