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_direct_transcription(self):
# Integrator.
plant = LinearSystem(A=[0.], B=[1.], C=[1.], D=[0.], time_period=0.1)
context = plant.CreateDefaultContext()
dirtran = DirectTranscription(plant, context, num_time_samples=21)
# Spell out most of the methods, regardless of whether they make sense
# as a consistent optimization. The goal is to check the bindings,
# not the implementation.
t = dirtran.time()
dt = dirtran.fixed_timestep()
x = dirtran.state()
x2 = dirtran.state(2)
x0 = dirtran.initial_state()
xf = dirtran.final_state()
u = dirtran.input()
u2 = dirtran.input(2)
dirtran.AddRunningCost(x.dot(x))
dirtran.AddConstraintToAllKnotPoints(u[0] == 0)
dirtran.AddFinalCost(2*x.dot(x))
initial_u = PiecewisePolynomial.ZeroOrderHold([0, .3*21],
np.zeros((1, 2)))
initial_x = PiecewisePolynomial()
dirtran.SetInitialTrajectory(initial_u, initial_x)
result = mp.Solve(dirtran)
times = dirtran.GetSampleTimes(result)
inputs = dirtran.GetInputSamples(result)
states = dirtran.GetStateSamples(result)
input_traj = dirtran.ReconstructInputTrajectory(result)
state_traj = dirtran.ReconstructStateTrajectory(result)
def test_direct_transcription_continuous_time(self):
# Test that the continuous-time constructor is also spelled correctly.
plant = LinearSystem(A=[0.], B=[1.], C=[1.], D=[0.])
context = plant.CreateDefaultContext()
dirtran = DirectTranscription(plant, context, num_time_samples=3,
fixed_timestep=TimeStep(0.1))
self.assertEqual(len(dirtran.linear_equality_constraints()), 3)
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_direct_transcription_continuous_time(self):
# Test that the continuous-time constructor is also spelled correctly.
plant = LinearSystem(A=[0.], B=[1.], C=[1.], D=[0.])
context = plant.CreateDefaultContext()
dirtran = DirectTranscription(plant,
context,
num_time_samples=3,
fixed_timestep=TimeStep(0.1))
with warnings.catch_warnings(record=True) as w:
warnings.simplefilter("once", DrakeDeprecationWarning)
self.assertEqual(len(dirtran.linear_equality_constraints()), 3)
self.assertEqual(len(dirtran.prog().linear_equality_constraints()), 3)
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)
