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)
# set the operating point (vertical unstable equilibrium)
context = cartpole.CreateDefaultContext()
context.get_mutable_continuous_state_vector().SetFromVector(x_star)
# fix the input port to zero and get its index for the lqr function
cartpole.get_actuation_input_port().FixValue(context, [0])
input_i = cartpole.get_actuation_input_port().get_index()
# synthesize lqr controller directly from
# the nonlinear system and the operating point
lqr = LinearQuadraticRegulator(cartpole, context, Q, R, input_port_index=input_i)
lqr = builder.AddSystem(lqr)
# the following two lines are not needed here...
output_i = cartpole.get_state_output_port().get_index()
cartpole_lin = Linearize(cartpole, context, input_port_index=input_i, output_port_index=output_i)
# wire cart-pole and lqr
builder.Connect(cartpole.get_state_output_port(), lqr.get_input_port(0))
builder.Connect(lqr.get_output_port(0), cartpole.get_actuation_input_port())
# add a visualizer and wire it
visualizer = builder.AddSystem(
PlanarSceneGraphVisualizer(scene_graph, xlim=[-3., 3.], ylim=[-1.2, 1.2], show=False)
)
builder.Connect(scene_graph.get_pose_bundle_output_port(), visualizer.get_input_port(0))
# finish building the block diagram
diagram = builder.Build()
# instantiate a simulator