def test_deprecated_math_prog_methods(self):
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)
dirtran.num_vars()
self.assertEqual(len(w), 1)
expected_message = ("Use trajopt.prog().num_vars(...) instead "
"of trajopt.num_vars(...).")
self.assertIn(expected_message, str(w[0].message))
# Test a small subset of the exhaustive list.
with warnings.catch_warnings(record=True) as w:
warnings.simplefilter("once", DrakeDeprecationWarning)
x = dirtran.NewContinuousVariables(3, 'x')
dirtran.AddCost(x[0] + 2)
dirtran.AddLinearCost(x[0])
dirtran.AddConstraint(x[1] == x[2])
self.assertEqual(len(w), 4)
with warnings.catch_warnings(record=True) as w:
warnings.simplefilter("once", DrakeDeprecationWarning)
result = mp.Solve(dirtran)
self.assertEqual(len(w), 1)
expected_message = (
"The trajectory optimization classes no longer derive from "
"MathematicalProgram. Use Solve(trajopt.prog()).")
self.assertIn(expected_message, str(w[0].message))
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.prog())
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_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 _do_test_direct_transcription(self, use_deprecated_solve):
# 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)
if use_deprecated_solve:
with warnings.catch_warnings(record=True) as w:
warnings.simplefilter('always', DrakeDeprecationWarning)
dirtran.Solve()
times = dirtran.GetSampleTimes()
inputs = dirtran.GetInputSamples()
states = dirtran.GetStateSamples()
input_traj = dirtran.ReconstructInputTrajectory()
state_traj = dirtran.ReconstructStateTrajectory()
self.assertEqual(len(w), 6)
else:
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_linear_system_zero_size(self):
# Explicitly test #12633.
num_x = 0
num_y = 2
num_u = 2
A = np.zeros((num_x, num_x))
B = np.zeros((num_x, num_u))
C = np.zeros((num_y, num_x))
D = np.zeros((num_y, num_u))
self.assertIsNotNone(LinearSystem(A, B, C, D))
def test_event_api(self):
# TriggerType - existence check.
TriggerType.kUnknown
TriggerType.kInitialization
TriggerType.kForced
TriggerType.kTimed
TriggerType.kPeriodic
TriggerType.kPerStep
TriggerType.kWitness
# PublishEvent.
# TODO(eric.cousineau): Test other event types when it is useful to
# expose them.
def callback(context, event):
pass
event = PublishEvent(callback=callback)
self.assertIsInstance(event, Event)
event = PublishEvent(trigger_type=TriggerType.kInitialization,
callback=callback)
self.assertIsInstance(event, Event)
self.assertEqual(event.get_trigger_type(), TriggerType.kInitialization)
def system_callback(system, context, event):
pass
event = PublishEvent(system_callback=system_callback)
self.assertIsInstance(event, Event)
event = PublishEvent(trigger_type=TriggerType.kInitialization,
system_callback=system_callback)
self.assertIsInstance(event, Event)
self.assertEqual(event.get_trigger_type(), TriggerType.kInitialization)
# Simple discrete-time system.
system1 = LinearSystem(A=[1], B=[1], C=[1], D=[1], time_period=0.1)
periodic_data = system1.GetUniquePeriodicDiscreteUpdateAttribute()
self.assertIsInstance(periodic_data, PeriodicEventData)
self.assertIsInstance(periodic_data.Clone(), PeriodicEventData)
periodic_data.period_sec()
periodic_data.offset_sec()
is_diff_eq, period = system1.IsDifferenceEquationSystem()
self.assertTrue(is_diff_eq)
self.assertEqual(period, periodic_data.period_sec())
# Simple continuous-time system.
system2 = LinearSystem(A=[1], B=[1], C=[1], D=[1], time_period=0.0)
periodic_data = system2.GetUniquePeriodicDiscreteUpdateAttribute()
self.assertIsNone(periodic_data)
is_diff_eq, period = system2.IsDifferenceEquationSystem()
self.assertFalse(is_diff_eq)
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.)
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)
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())
