Controllers

Detailed Description

Implementations of controllers that operate as Systems in a block diagram.

Algorithms that synthesize controllers are located in Feedback Control Design.

Classes
class	InverseDynamicsController< T >
	A state feedback controller that uses a PidController to generate desired accelerations, which are then converted into torques using InverseDynamics. More...
class	LinearModelPredictiveController< T >
	Implements a basic Model Predictive Controller that linearizes the system about an equilibrium condition and regulates to the same point by solving an optimal control problem over a finite time horizon. More...
class	PidControlledSystem< T >
	A system that encapsulates a PidController and a controlled System (a.k.a the "plant"). More...
class	PidController< T >
	Implements the PID controller. More...

Functions
std::pair< std::unique_ptr< BarycentricMeshSystem< double > >, Eigen::RowVectorXd >	FittedValueIteration (Simulator< double > *simulator, const std::function< double(const Context< double > &context)> &cost_function, const math::BarycentricMesh< double >::MeshGrid &state_grid, const math::BarycentricMesh< double >::MeshGrid &input_grid, double timestep, const DynamicProgrammingOptions &options=DynamicProgrammingOptions())
	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. More...
Eigen::VectorXd	LinearProgrammingApproximateDynamicProgramming (Simulator< double > *simulator, const std::function< double(const Context< double > &context)> &cost_function, const std::function< symbolic::Expression(const Eigen::Ref< const Eigen::VectorXd > &state, const VectorX< symbolic::Variable > &parameters)> &linearly_parameterized_cost_to_go_function, int num_parameters, const Eigen::Ref< const Eigen::MatrixXd > &state_samples, const Eigen::Ref< const Eigen::MatrixXd > &input_samples, double timestep, const DynamicProgrammingOptions &options=DynamicProgrammingOptions())
	Implements the Linear Programming approach to approximate dynamic programming. More...
FiniteHorizonLinearQuadraticRegulatorResult	FiniteHorizonLinearQuadraticRegulator (const System< double > &system, const Context< double > &context, double t0, double tf, const Eigen::Ref< const Eigen::MatrixXd > &Q, const Eigen::Ref< const Eigen::MatrixXd > &R, const FiniteHorizonLinearQuadraticRegulatorOptions &options=FiniteHorizonLinearQuadraticRegulatorOptions())
	Solves the differential Riccati equation to compute the optimal controller and optimal cost-to-go for the finite-horizon linear quadratic regulator: More...
std::unique_ptr< System< double > >	MakeFiniteHorizonLinearQuadraticRegulator (const System< double > &system, const Context< double > &context, double t0, double tf, const Eigen::Ref< const Eigen::MatrixXd > &Q, const Eigen::Ref< const Eigen::MatrixXd > &R, const FiniteHorizonLinearQuadraticRegulatorOptions &options=FiniteHorizonLinearQuadraticRegulatorOptions())
	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. More...
LinearQuadraticRegulatorResult	LinearQuadraticRegulator (const Eigen::Ref< const Eigen::MatrixXd > &A, const Eigen::Ref< const Eigen::MatrixXd > &B, const Eigen::Ref< const Eigen::MatrixXd > &Q, const Eigen::Ref< const Eigen::MatrixXd > &R, const Eigen::Ref< const Eigen::MatrixXd > &N=Eigen::Matrix< double, 0, 0 >::Zero())
	Computes the optimal feedback controller, u=-Kx, and the optimal cost-to-go J = x'Sx for the problem: More...
LinearQuadraticRegulatorResult	DiscreteTimeLinearQuadraticRegulator (const Eigen::Ref< const Eigen::MatrixXd > &A, const Eigen::Ref< const Eigen::MatrixXd > &B, const Eigen::Ref< const Eigen::MatrixXd > &Q, const Eigen::Ref< const Eigen::MatrixXd > &R)
	Computes the optimal feedback controller, u=-Kx, and the optimal cost-to-go J = x'Sx for the problem: More...
std::unique_ptr< LinearSystem< double > >	LinearQuadraticRegulator (const LinearSystem< double > &system, const Eigen::Ref< const Eigen::MatrixXd > &Q, const Eigen::Ref< const Eigen::MatrixXd > &R, const Eigen::Ref< const Eigen::MatrixXd > &N=Eigen::Matrix< double, 0, 0 >::Zero())
	Creates a system that implements the optimal time-invariant linear quadratic regulator (LQR). More...
std::unique_ptr< AffineSystem< double > >	LinearQuadraticRegulator (const System< double > &system, const Context< double > &context, const Eigen::Ref< const Eigen::MatrixXd > &Q, const Eigen::Ref< const Eigen::MatrixXd > &R, const Eigen::Ref< const Eigen::MatrixXd > &N=Eigen::Matrix< double, 0, 0 >::Zero(), int input_port_index=0)
	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. More...
Eigen::MatrixXd	ControllabilityMatrix (const LinearSystem< double > &sys)
	Returns the controllability matrix: R = [B, AB, ..., A^{n-1}B]. More...
bool	IsControllable (const LinearSystem< double > &sys, std::optional< double > threshold=std::nullopt)
	Returns true iff the controllability matrix is full row rank. More...

Function Documentation

ControllabilityMatrix()

Eigen::MatrixXd drake::systems::ControllabilityMatrix

(

const LinearSystem< double > &

sys

)

Returns the controllability matrix: R = [B, AB, ..., A^{n-1}B].

DiscreteTimeLinearQuadraticRegulator()

LinearQuadraticRegulatorResult drake::systems::controllers::DiscreteTimeLinearQuadraticRegulator	(	const Eigen::Ref< const Eigen::MatrixXd > &	A,
		const Eigen::Ref< const Eigen::MatrixXd > &	B,
		const Eigen::Ref< const Eigen::MatrixXd > &	Q,
		const Eigen::Ref< const Eigen::MatrixXd > &	R
	)

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 \]

Parameters

A	The state-space dynamics matrix of size num_states x num_states.
B	The state-space input matrix of size num_states x num_inputs.
Q	A symmetric positive semi-definite cost matrix of size num_states x num_states.
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;

Exceptions

std::runtime_error

if R is not positive definite.

FiniteHorizonLinearQuadraticRegulator()

FiniteHorizonLinearQuadraticRegulatorResult drake::systems::controllers::FiniteHorizonLinearQuadraticRegulator	(	const System< double > &	system,
		const Context< double > &	context,
		double	t0,
		double	tf,
		const Eigen::Ref< const Eigen::MatrixXd > &	Q,
		const Eigen::Ref< const Eigen::MatrixXd > &	R,
		const FiniteHorizonLinearQuadraticRegulatorOptions &	options = FiniteHorizonLinearQuadraticRegulatorOptions()
	)

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 by context.

Parameters

system	a System<double> representing the plant.
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.
t0	is the initial time.
tf	is the final time (with tf > t0).
Q	is nxn positive semi-definite.
R	is mxm positive definite.
options	is the optional FiniteHorizonLinearQuadraticRegulatorOptions.

Precondition

system must be a System<double> with (only) n continuous state variables and m inputs. It must be convertable 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).

FittedValueIteration()

std::pair<std::unique_ptr<BarycentricMeshSystem<double> >, Eigen::RowVectorXd> drake::systems::controllers::FittedValueIteration	(	Simulator< double > *	simulator,
		const std::function< double(const Context< double > &context)> &	cost_function,
		const math::BarycentricMesh< double >::MeshGrid &	state_grid,
		const math::BarycentricMesh< double >::MeshGrid &	input_grid,
		double	timestep,
		const DynamicProgrammingOptions &	options = DynamicProgrammingOptions()
	)

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 timestep) for the value iteration update.

Parameters

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 for timestep 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.
cost_function	is the continuous-time instantaneous cost. This implementation of the discrete-time formulation above uses the approximation g(x,u) = timestep*cost_function(x,u).
state_grid	defines the mesh on the state space used to represent the cost-to-go function and the resulting policy.
input_grid	defines the discrete action space used in the value iteration update.
timestep	a time in seconds used for the discrete-time approximation.
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 through simulator) and a single vector output (which is the input of the system passed in through simulator).

IsControllable()

bool drake::systems::IsControllable	(	const LinearSystem< double > &	sys,
		std::optional< double >	threshold = std::nullopt
	)

Returns true iff the controllability matrix is full row rank.

LinearProgrammingApproximateDynamicProgramming()

Eigen::VectorXd drake::systems::controllers::LinearProgrammingApproximateDynamicProgramming	(	Simulator< double > *	simulator,
		const std::function< double(const Context< double > &context)> &	cost_function,
		const std::function< symbolic::Expression(const Eigen::Ref< const Eigen::VectorXd > &state, const VectorX< symbolic::Variable > &parameters)> &	linearly_parameterized_cost_to_go_function,
		int	num_parameters,
		const Eigen::Ref< const Eigen::MatrixXd > &	state_samples,
		const Eigen::Ref< const Eigen::MatrixXd > &	input_samples,
		double	timestep,
		const DynamicProgrammingOptions &	options = DynamicProgrammingOptions()
	)

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 timestep) for the value iteration update.

Parameters

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 for timestep 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.
cost_function	is the continuous-time instantaneous cost. This implementation of the discrete-time formulation above uses the approximation g(x,u) = timestep*cost_function(x,u).
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.
state_samples	is a list of sample states (one per column) at which to apply the optimization constraints and the objective.
input_samples	is a list of inputs (one per column) which are evaluated at every sample point.
timestep	a time in seconds used for the discrete-time approximation.
options	optional DynamicProgrammingOptions structure.

Returns

params the VectorXd of parameters that optimizes the supplied cost-to-go function.

LinearQuadraticRegulator() [1/3]

LinearQuadraticRegulatorResult drake::systems::controllers::LinearQuadraticRegulator	(	const Eigen::Ref< const Eigen::MatrixXd > &	A,
		const Eigen::Ref< const Eigen::MatrixXd > &	B,
		const Eigen::Ref< const Eigen::MatrixXd > &	Q,
		const Eigen::Ref< const Eigen::MatrixXd > &	R,
		const Eigen::Ref< const Eigen::MatrixXd > &	N = Eigen::Matrix< double, 0, 0 >::Zero()
	)

Computes the optimal feedback controller, u=-Kx, and the optimal cost-to-go J = x'Sx for the problem:

\[ \dot{x} = Ax + Bu \]

\[ \min_u \int_0^\infty x'Qx + u'Ru + 2x'Nu dt \]

Parameters

A	The state-space dynamics matrix of size num_states x num_states.
B	The state-space input matrix of size num_states x num_inputs.
Q	A symmetric positive semi-definite cost matrix of size num_states x num_states.
R	A symmetric positive definite cost matrix of size num_inputs x num_inputs.
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.

Returns

A structure that contains the optimal feedback gain K and the quadratic cost term S. The optimal feedback control is u = -Kx;

Exceptions

std::runtime_error

if R is not positive definite.

LinearQuadraticRegulator() [2/3]

std::unique_ptr<LinearSystem<double> > drake::systems::controllers::LinearQuadraticRegulator	(	const LinearSystem< double > &	system,
		const Eigen::Ref< const Eigen::MatrixXd > &	Q,
		const Eigen::Ref< const Eigen::MatrixXd > &	R,
		const Eigen::Ref< const Eigen::MatrixXd > &	N = Eigen::Matrix< double, 0, 0 >::Zero()
	)

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) 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]. \]

Parameters

system	The System to be controlled.
Q	A symmetric positive semi-definite cost matrix of size num_states x num_states.
R	A symmetric positive definite cost matrix of size num_inputs x num_inputs.
N	A cost matrix of size num_states x num_inputs.

Returns

A system implementing the optimal controller in the original system coordinates.

Exceptions

std::runtime_error

if R is not positive definite.

LinearQuadraticRegulator() [3/3]

std::unique_ptr<AffineSystem<double> > drake::systems::controllers::LinearQuadraticRegulator	(	const System< double > &	system,
		const Context< double > &	context,
		const Eigen::Ref< const Eigen::MatrixXd > &	Q,
		const Eigen::Ref< const Eigen::MatrixXd > &	R,
		const Eigen::Ref< const Eigen::MatrixXd > &	N = Eigen::Matrix< double, 0, 0 >::Zero(),
		int	input_port_index = 0
	)

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) 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), \]

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.

Parameters

system	The System to be controlled.
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.
Q	A symmetric positive semi-definite cost matrix of size num_states x num_states.
R	A symmetric positive definite cost matrix of size num_inputs x num_inputs.
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.
int_port_index	The index of the input port to linearize around.

Returns

A system implementing the optimal controller in the original system coordinates.

Exceptions

std::runtime_error

if R is not positive definite.

See also

drake::systems::Linearize()

MakeFiniteHorizonLinearQuadraticRegulator()

std::unique_ptr<System<double> > drake::systems::controllers::MakeFiniteHorizonLinearQuadraticRegulator	(	const System< double > &	system,
		const Context< double > &	context,
		double	t0,
		double	tf,
		const Eigen::Ref< const Eigen::MatrixXd > &	Q,
		const Eigen::Ref< const Eigen::MatrixXd > &	R,
		const FiniteHorizonLinearQuadraticRegulatorOptions &	options = FiniteHorizonLinearQuadraticRegulatorOptions()
	)

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.