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