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)
def test_simulation(self):
# Basic constant-torque pendulum simulation.
pendulum = PendulumPlant()
# Create the simulator.
simulator = Simulator(pendulum)
context = simulator.get_mutable_context()
# Set an input torque.
input = PendulumInput()
input.set_tau(1.)
context.FixInputPort(0, input)
# Set the initial state.
state = context.get_mutable_continuous_state_vector()
state.set_theta(1.)
state.set_thetadot(0.)
# Simulate (and make sure the state actually changes).
initial_state = state.CopyToVector()
simulator.AdvanceTo(1.0)
self.assertFalse((state.CopyToVector() == initial_state).any())
def test_simulation(self):
# Basic constant-torque pendulum simulation.
pendulum = PendulumPlant()
# Create the simulator.
simulator = Simulator(pendulum)
context = simulator.get_mutable_context()
# Set an input torque.
input = PendulumInput()
input.set_tau(1.)
context.FixInputPort(0, input)
# Set the initial state.
state = context.get_mutable_continuous_state_vector()
state.set_theta(1.)
state.set_thetadot(0.)
# Simulate (and make sure the state actually changes).
initial_state = state.CopyToVector()
simulator.StepTo(1.0)
self.assertFalse((state.CopyToVector() == initial_state).any())
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)
def test_input(self):
input = PendulumInput()
input.set_tau(1.)
self.assertEqual(input.tau(), 1.)
def test_input(self):
input = PendulumInput()
input.set_tau(1.)
self.assertEqual(input.tau(), 1.)
def test_input(self):
input = PendulumInput()
input.set_tau(tau=1.)
self.assertEqual(input.tau(), 1.)
input_tau = input.with_tau(tau=2.)
self.assertEqual(input_tau.tau(), 2.)