def get_A_B(t):
context = plant.CreateDefaultContext()
context.SetContinuousState(x_traj.eval(t, 0))
context.FixInputPort(0, u_traj.eval(t, 0))
linear_system = Linearize(plant,
context,
equilibrium_check_tolerance=np.inf)
return linear_system.A(), linear_system.B()
def get_LQR_params(self):
quadroter = QuadrotorPlant()
context = quadroter.CreateDefaultContext()
hover_thrust = self.m * self.g
quadroter.get_input_port(0).FixValue(context, [
hover_thrust / 4.0,
] * 4)
context.get_mutable_continuous_state_vector()\
.SetFromVector(self.nominal_state)
linear_sys = Linearize(quadroter, context, InputPortIndex(0),
OutputPortIndex(0))
return LinearQuadraticRegulator(linear_sys.A(), linear_sys.B(), self.Q,
self.R)
def BalancingLQR():
# Design an LQR controller for stabilizing the Acrobot around the upright.
# Returns a (static) AffineSystem that implements the controller (in
# the original AcrobotState coordinates).
pendulum = PendulumPlant()
context = pendulum.CreateDefaultContext()
input = PendulumInput()
input.set_tau(0.)
context.FixInputPort(0, input)
context.get_mutable_continuous_state_vector().SetFromVector(
UprightState().CopyToVector())
Q = np.diag((10., 1.))
R = [1]
linearized_pendulum = Linearize(pendulum, context)
(K, S) = LinearQuadraticRegulator(linearized_pendulum.A(),
linearized_pendulum.B(), Q, R)
return (K, S)