def test_finite_horizon_linear_quadratic_regulator(self):
A = np.array([[0, 1], [0, 0]])
B = np.array([[0], [1]])
C = np.identity(2)
D = np.array([[0], [0]])
double_integrator = LinearSystem(A, B, C, D)
Q = np.identity(2)
R = np.identity(1)
options = FiniteHorizonLinearQuadraticRegulatorOptions()
options.Qf = Q
self.assertIsNone(options.N)
self.assertIsNone(options.x0)
self.assertIsNone(options.u0)
self.assertIsNone(options.xd)
self.assertIsNone(options.ud)
self.assertEqual(options.input_port_index,
InputPortSelection.kUseFirstInputIfItExists)
context = double_integrator.CreateDefaultContext()
double_integrator.get_input_port(0).FixValue(context, 0.0)
result = FiniteHorizonLinearQuadraticRegulator(
system=double_integrator,
context=context,
t0=0,
tf=0.1,
Q=Q,
R=R,
options=options)
self.assertIsInstance(result,
FiniteHorizonLinearQuadraticRegulatorResult)
self.assertIsInstance(result.x0, Trajectory)
self.assertEqual(result.x0.value(0).shape, (2, 1))
self.assertIsInstance(result.u0, Trajectory)
self.assertEqual(result.u0.value(0).shape, (1, 1))
self.assertIsInstance(result.K, Trajectory)
self.assertEqual(result.K.value(0).shape, (1, 2))
self.assertIsInstance(result.S, Trajectory)
self.assertEqual(result.S.value(0).shape, (2, 2))
self.assertIsInstance(result.k0, Trajectory)
self.assertEqual(result.k0.value(0).shape, (1, 1))
self.assertIsInstance(result.sx, Trajectory)
self.assertEqual(result.sx.value(0).shape, (2, 1))
self.assertIsInstance(result.s0, Trajectory)
self.assertEqual(result.s0.value(0).shape, (1, 1))
regulator = MakeFiniteHorizonLinearQuadraticRegulator(
system=double_integrator,
context=context,
t0=0,
tf=0.1,
Q=Q,
R=R,
options=options)
self.assertEqual(regulator.get_input_port(0).size(), 2)
self.assertEqual(regulator.get_output_port(0).size(), 1)
def test_linear_quadratic_regulator(self):
A = np.array([[0, 1], [0, 0]])
B = np.array([[0], [1]])
C = np.identity(2)
D = np.array([[0], [0]])
double_integrator = LinearSystem(A, B, C, D)
Q = np.identity(2)
R = np.identity(1)
K_expected = np.array([[1, math.sqrt(3.)]])
S_expected = np.array([[math.sqrt(3), 1.], [1., math.sqrt(3)]])
(K, S) = LinearQuadraticRegulator(A, B, Q, R)
np.testing.assert_almost_equal(K, K_expected)
np.testing.assert_almost_equal(S, S_expected)
controller = LinearQuadraticRegulator(double_integrator, Q, R)
np.testing.assert_almost_equal(controller.D(), -K_expected)
context = double_integrator.CreateDefaultContext()
double_integrator.get_input_port(0).FixValue(context, [0])
controller = LinearQuadraticRegulator(
double_integrator,
context,
Q,
R,
input_port_index=double_integrator.get_input_port().get_index())
np.testing.assert_almost_equal(controller.D(), -K_expected)
def test_linear_affine_system(self):
# Just make sure linear system is spelled correctly.
A = np.identity(2)
B = np.array([[0], [1]])
f0 = np.array([[0], [0]])
C = np.array([[0, 1]])
D = [0]
y0 = [0]
system = LinearSystem(A, B, C, D)
context = system.CreateDefaultContext()
self.assertEqual(system.get_input_port(0).size(), 1)
self.assertEqual(context.get_mutable_continuous_state_vector().size(),
2)
self.assertEqual(system.get_output_port(0).size(), 1)
self.assertTrue((system.A() == A).all())
self.assertTrue((system.B() == B).all())
self.assertTrue((system.f0() == f0).all())
self.assertTrue((system.C() == C).all())
self.assertEqual(system.D(), D)
self.assertEqual(system.y0(), y0)
self.assertEqual(system.time_period(), 0.)
Co = ControllabilityMatrix(system)
self.assertEqual(Co.shape, (2, 2))
self.assertFalse(IsControllable(system))
self.assertFalse(IsControllable(system, 1e-6))
Ob = ObservabilityMatrix(system)
self.assertEqual(Ob.shape, (2, 2))
self.assertFalse(IsObservable(system))
system = AffineSystem(A, B, f0, C, D, y0, .1)
context = system.CreateDefaultContext()
self.assertEqual(system.get_input_port(0).size(), 1)
self.assertEqual(context.get_discrete_state_vector().size(), 2)
self.assertEqual(system.get_output_port(0).size(), 1)
self.assertTrue((system.A() == A).all())
self.assertTrue((system.B() == B).all())
self.assertTrue((system.f0() == f0).all())
self.assertTrue((system.C() == C).all())
self.assertEqual(system.D(), D)
self.assertEqual(system.y0(), y0)
self.assertEqual(system.time_period(), .1)
context.FixInputPort(0, BasicVector([0]))
linearized = Linearize(system, context)
self.assertTrue((linearized.A() == A).all())
taylor = FirstOrderTaylorApproximation(system, context)
self.assertTrue((taylor.y0() == y0).all())
system = MatrixGain(D=A)
self.assertTrue((system.D() == A).all())
def test_linear_affine_system(self):
# Just make sure linear system is spelled correctly.
A = np.identity(2)
B = np.array([[0], [1]])
f0 = np.array([[0], [0]])
C = np.array([[0, 1]])
D = [0]
y0 = [0]
system = LinearSystem(A, B, C, D)
context = system.CreateDefaultContext()
self.assertEqual(system.get_input_port(0).size(), 1)
self.assertEqual(context
.get_mutable_continuous_state_vector().size(), 2)
self.assertEqual(system.get_output_port(0).size(), 1)
self.assertTrue((system.A() == A).all())
self.assertTrue((system.B() == B).all())
self.assertTrue((system.f0() == f0).all())
self.assertTrue((system.C() == C).all())
self.assertEqual(system.D(), D)
self.assertEqual(system.y0(), y0)
self.assertEqual(system.time_period(), 0.)
Co = ControllabilityMatrix(system)
self.assertEqual(Co.shape, (2, 2))
self.assertFalse(IsControllable(system))
self.assertFalse(IsControllable(system, 1e-6))
Ob = ObservabilityMatrix(system)
self.assertEqual(Ob.shape, (2, 2))
self.assertFalse(IsObservable(system))
system = AffineSystem(A, B, f0, C, D, y0, .1)
context = system.CreateDefaultContext()
self.assertEqual(system.get_input_port(0).size(), 1)
self.assertEqual(context.get_discrete_state_vector().size(), 2)
self.assertEqual(system.get_output_port(0).size(), 1)
self.assertTrue((system.A() == A).all())
self.assertTrue((system.B() == B).all())
self.assertTrue((system.f0() == f0).all())
self.assertTrue((system.C() == C).all())
self.assertEqual(system.D(), D)
self.assertEqual(system.y0(), y0)
self.assertEqual(system.time_period(), .1)
context.FixInputPort(0, BasicVector([0]))
linearized = Linearize(system, context)
self.assertTrue((linearized.A() == A).all())
taylor = FirstOrderTaylorApproximation(system, context)
self.assertTrue((taylor.y0() == y0).all())
system = MatrixGain(D=A)
self.assertTrue((system.D() == A).all())
def test_linear_affine_system(self):
# Just make sure linear system is spelled correctly.
A = np.identity(2)
B = np.array([[0], [1]])
f0 = np.array([[0], [0]])
C = np.array([[0, 1]])
D = [0]
y0 = [0]
system = LinearSystem(A, B, C, D)
context = system.CreateDefaultContext()
self.assertEqual(system.get_input_port(0).size(), 1)
self.assertEqual(context.get_mutable_continuous_state_vector().size(),
2)
self.assertEqual(system.get_output_port(0).size(), 1)
self.assertTrue((system.A() == A).all())
self.assertTrue((system.B() == B).all())
self.assertTrue((system.f0() == f0).all())
self.assertTrue((system.C() == C).all())
self.assertEqual(system.D(), D)
self.assertEqual(system.y0(), y0)
self.assertEqual(system.time_period(), 0.)
x0 = np.array([1, 2])
system.configure_default_state(x0=x0)
system.SetDefaultContext(context)
np.testing.assert_equal(
context.get_continuous_state_vector().CopyToVector(), x0)
generator = RandomGenerator()
system.SetRandomContext(context, generator)
np.testing.assert_equal(
context.get_continuous_state_vector().CopyToVector(), x0)
system.configure_random_state(covariance=np.eye(2))
system.SetRandomContext(context, generator)
self.assertNotEqual(
context.get_continuous_state_vector().CopyToVector()[1], x0[1])
Co = ControllabilityMatrix(system)
self.assertEqual(Co.shape, (2, 2))
self.assertFalse(IsControllable(system))
self.assertFalse(IsControllable(system, 1e-6))
Ob = ObservabilityMatrix(system)
self.assertEqual(Ob.shape, (2, 2))
self.assertFalse(IsObservable(system))
system = AffineSystem(A, B, f0, C, D, y0, .1)
self.assertEqual(system.get_input_port(0), system.get_input_port())
self.assertEqual(system.get_output_port(0), system.get_output_port())
context = system.CreateDefaultContext()
self.assertEqual(system.get_input_port(0).size(), 1)
self.assertEqual(context.get_discrete_state_vector().size(), 2)
self.assertEqual(system.get_output_port(0).size(), 1)
self.assertTrue((system.A() == A).all())
self.assertTrue((system.B() == B).all())
self.assertTrue((system.f0() == f0).all())
self.assertTrue((system.C() == C).all())
self.assertEqual(system.D(), D)
self.assertEqual(system.y0(), y0)
self.assertEqual(system.time_period(), .1)
system.get_input_port(0).FixValue(context, 0)
linearized = Linearize(system, context)
self.assertTrue((linearized.A() == A).all())
taylor = FirstOrderTaylorApproximation(system, context)
self.assertTrue((taylor.y0() == y0).all())
system = MatrixGain(D=A)
self.assertTrue((system.D() == A).all())
def test_finite_horizon_linear_quadratic_regulator(self):
A = np.array([[0, 1], [0, 0]])
B = np.array([[0], [1]])
C = np.identity(2)
D = np.array([[0], [0]])
double_integrator = LinearSystem(A, B, C, D)
Q = np.identity(2)
R = np.identity(1)
options = FiniteHorizonLinearQuadraticRegulatorOptions()
options.Qf = Q
options.use_square_root_method = False
self.assertIsNone(options.N)
self.assertIsNone(options.x0)
self.assertIsNone(options.u0)
self.assertIsNone(options.xd)
self.assertIsNone(options.ud)
self.assertEqual(options.input_port_index,
InputPortSelection.kUseFirstInputIfItExists)
self.assertRegex(
repr(options),
"".join([
r"FiniteHorizonLinearQuadraticRegulatorOptions\(",
# Don't be particular about numpy's whitespace in Qf.
r"Qf=\[\[ *1\. *0\.\]\s*\[ *0\. *1\.\]\], "
r"N=None, ",
r"input_port_index=",
r"InputPortSelection.kUseFirstInputIfItExists, ",
r"use_square_root_method=False\)"
]))
context = double_integrator.CreateDefaultContext()
double_integrator.get_input_port(0).FixValue(context, 0.0)
result = FiniteHorizonLinearQuadraticRegulator(
system=double_integrator,
context=context,
t0=0,
tf=0.1,
Q=Q,
R=R,
options=options)
self.assertIsInstance(result,
FiniteHorizonLinearQuadraticRegulatorResult)
self.assertIsInstance(result.x0, Trajectory)
self.assertEqual(result.x0.value(0).shape, (2, 1))
self.assertIsInstance(result.u0, Trajectory)
self.assertEqual(result.u0.value(0).shape, (1, 1))
self.assertIsInstance(result.K, Trajectory)
self.assertEqual(result.K.value(0).shape, (1, 2))
self.assertIsInstance(result.S, Trajectory)
self.assertEqual(result.S.value(0).shape, (2, 2))
self.assertIsInstance(result.k0, Trajectory)
self.assertEqual(result.k0.value(0).shape, (1, 1))
self.assertIsInstance(result.sx, Trajectory)
self.assertEqual(result.sx.value(0).shape, (2, 1))
self.assertIsInstance(result.s0, Trajectory)
self.assertEqual(result.s0.value(0).shape, (1, 1))
regulator = MakeFiniteHorizonLinearQuadraticRegulator(
system=double_integrator,
context=context,
t0=0,
tf=0.1,
Q=Q,
R=R,
options=options)
self.assertEqual(regulator.get_input_port(0).size(), 2)
self.assertEqual(regulator.get_output_port(0).size(), 1)