def test_signal_logger(self):
# Log the output of a simple diagram containing a constant
# source and an integrator.
builder = DiagramBuilder()
kValue = 2.4
source = builder.AddSystem(ConstantVectorSource([kValue]))
kSize = 1
integrator = builder.AddSystem(Integrator(kSize))
logger = builder.AddSystem(SignalLogger(kSize))
builder.Connect(source.get_output_port(0),
integrator.get_input_port(0))
builder.Connect(integrator.get_output_port(0),
logger.get_input_port(0))
# Add a redundant logger via the helper method.
logger2 = LogOutput(integrator.get_output_port(0), builder)
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.StepTo(1)
t = logger.sample_times()
x = logger.data()
self.assertTrue(t.shape[0] > 2)
self.assertTrue(t.shape[0] == x.shape[1])
self.assertAlmostEqual(x[0, -1], t[-1] * kValue, places=2)
np.testing.assert_array_equal(x, logger2.data())
logger.reset()
def test_linear_programming_approximate_dynamic_programming(self):
integrator = Integrator(1)
simulator = Simulator(integrator)
# minimum time cost function (1 for all non-zero states).
def cost_function(context):
x = context.get_continuous_state_vector().CopyToVector()
if (math.fabs(x[0]) > 0.1):
return 1.
else:
return 0.
def cost_to_go_function(state, parameters):
return parameters[0] * math.fabs(state[0])
state_samples = np.array([[-4., -3., -2., -1., 0., 1., 2., 3., 4.]])
input_samples = np.array([[-1., 0., 1.]])
timestep = 1.0
options = DynamicProgrammingOptions()
options.discount_factor = 1.
J = LinearProgrammingApproximateDynamicProgramming(
simulator, cost_function, cost_to_go_function, 1,
state_samples, input_samples, timestep, options)
self.assertAlmostEqual(J[0], 1., delta=1e-6)
def test_signal_logger(self):
# Log the output of a simple diagram containing a constant
# source and an integrator.
builder = DiagramBuilder()
kValue = 2.4
source = builder.AddSystem(ConstantVectorSource([kValue]))
kSize = 1
integrator = builder.AddSystem(Integrator(kSize))
logger_per_step = builder.AddSystem(SignalLogger(kSize))
builder.Connect(source.get_output_port(0),
integrator.get_input_port(0))
builder.Connect(integrator.get_output_port(0),
logger_per_step.get_input_port(0))
# Add a redundant logger via the helper method.
logger_per_step_2 = LogOutput(integrator.get_output_port(0), builder)
# Add a periodic logger
logger_periodic = builder.AddSystem(SignalLogger(kSize))
kPeriod = 0.1
logger_periodic.set_publish_period(kPeriod)
builder.Connect(integrator.get_output_port(0),
logger_periodic.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
kTime = 1.
simulator.AdvanceTo(kTime)
# Verify outputs of the every-step logger
t = logger_per_step.sample_times()
x = logger_per_step.data()
self.assertTrue(t.shape[0] > 2)
self.assertTrue(t.shape[0] == x.shape[1])
self.assertAlmostEqual(x[0, -1], t[-1] * kValue, places=2)
np.testing.assert_array_equal(x, logger_per_step_2.data())
# Verify outputs of the periodic logger
t = logger_periodic.sample_times()
x = logger_periodic.data()
# Should log exactly once every kPeriod, up to and including kTime.
self.assertTrue(t.shape[0] == np.floor(kTime / kPeriod) + 1.)
logger_per_step.reset()
# Verify that t and x retain their values after systems are deleted.
t_copy = t.copy()
x_copy = x.copy()
del builder
del integrator
del logger_periodic
del logger_per_step
del logger_per_step_2
del diagram
del simulator
del source
gc.collect()
self.assertTrue((t == t_copy).all())
self.assertTrue((x == x_copy).all())
def test_str(self):
"""
Tests str() methods. See ./value_test.py for testing str() and
repr() specific to BasicVector.
"""
# Context.
integrator = Integrator(3)
integrator.set_name("integrator")
context = integrator.CreateDefaultContext()
# N.B. This is only to show behavior of C++ string formatting in
# Python. It is OK to update this when the upstream C++ code changes.
self.assertEqual(
str(context),
dedent("""\
::integrator Context
---------------------
Time: 0
States:
3 continuous states
0 0 0
"""),
)
def test_diagram_simulation(self):
# Similar to: //systems/framework:diagram_test, ExampleDiagram
size = 3
builder = DiagramBuilder()
adder0 = builder.AddSystem(Adder(2, size))
adder0.set_name("adder0")
adder1 = builder.AddSystem(Adder(2, size))
adder1.set_name("adder1")
integrator = builder.AddSystem(Integrator(size))
integrator.set_name("integrator")
builder.Connect(adder0.get_output_port(0), adder1.get_input_port(0))
builder.Connect(adder1.get_output_port(0),
integrator.get_input_port(0))
builder.ExportInput(adder0.get_input_port(0))
builder.ExportInput(adder0.get_input_port(1))
builder.ExportInput(adder1.get_input_port(1))
builder.ExportOutput(integrator.get_output_port(0))
diagram = builder.Build()
# TODO(eric.cousineau): Figure out unicode handling if needed.
# See //systems/framework/test/diagram_test.cc:349 (sha: bc84e73)
# for an example name.
diagram.set_name("test_diagram")
simulator = Simulator(diagram)
context = simulator.get_mutable_context()
# Create and attach inputs.
# TODO(eric.cousineau): Not seeing any assertions being printed if no
# inputs are connected. Need to check this behavior.
input0 = np.array([0.1, 0.2, 0.3])
context.FixInputPort(0, input0)
input1 = np.array([0.02, 0.03, 0.04])
context.FixInputPort(1, input1)
input2 = BasicVector([0.003, 0.004, 0.005])
context.FixInputPort(2, input2) # Test the BasicVector overload.
# Initialize integrator states.
integrator_xc = (
diagram.GetMutableSubsystemState(integrator, context)
.get_mutable_continuous_state().get_vector())
integrator_xc.SetFromVector([0, 1, 2])
simulator.Initialize()
# Simulate briefly, and take full-context snapshots at intermediate
# points.
n = 6
times = np.linspace(0, 1, n)
context_log = []
for t in times:
simulator.StepTo(t)
# Record snapshot of *entire* context.
context_log.append(context.Clone())
xc_initial = np.array([0, 1, 2])
xc_final = np.array([0.123, 1.234, 2.345])
for i, context_i in enumerate(context_log):
t = times[i]
self.assertEqual(context_i.get_time(), t)
xc = context_i.get_continuous_state_vector().CopyToVector()
xc_expected = (float(i) / (n - 1) * (xc_final - xc_initial) +
xc_initial)
print("xc[t = {}] = {}".format(t, xc))
self.assertTrue(np.allclose(xc, xc_expected))
def test_diagram_simulation(self):
# TODO(eric.cousineau): Move this to `analysis_test.py`.
# Similar to: //systems/framework:diagram_test, ExampleDiagram
size = 3
builder = DiagramBuilder()
self.assertTrue(builder.empty())
adder0 = builder.AddSystem(Adder(2, size))
adder0.set_name("adder0")
self.assertFalse(builder.empty())
adder1 = builder.AddSystem(Adder(2, size))
adder1.set_name("adder1")
integrator = builder.AddSystem(Integrator(size))
integrator.set_name("integrator")
self.assertEqual(
builder.GetMutableSystems(),
[adder0, adder1, integrator])
builder.Connect(adder0.get_output_port(0), adder1.get_input_port(0))
builder.Connect(adder1.get_output_port(0),
integrator.get_input_port(0))
# Exercise naming variants.
builder.ExportInput(adder0.get_input_port(0))
builder.ExportInput(adder0.get_input_port(1), kUseDefaultName)
builder.ExportInput(adder1.get_input_port(1), "third_input")
builder.ExportOutput(integrator.get_output_port(0), "result")
diagram = builder.Build()
self.assertEqual(adder0.get_name(), "adder0")
self.assertEqual(diagram.GetSubsystemByName("adder0"), adder0)
self.assertEqual(
diagram.GetSystems(),
[adder0, adder1, integrator])
# TODO(eric.cousineau): Figure out unicode handling if needed.
# See //systems/framework/test/diagram_test.cc:349 (sha: bc84e73)
# for an example name.
diagram.set_name("test_diagram")
simulator = Simulator(diagram)
context = simulator.get_mutable_context()
# Create and attach inputs.
# TODO(eric.cousineau): Not seeing any assertions being printed if no
# inputs are connected. Need to check this behavior.
input0 = np.array([0.1, 0.2, 0.3])
diagram.get_input_port(0).FixValue(context, input0)
input1 = np.array([0.02, 0.03, 0.04])
diagram.get_input_port(1).FixValue(context, input1)
# Test the BasicVector overload.
input2 = BasicVector([0.003, 0.004, 0.005])
diagram.get_input_port(2).FixValue(context, input2)
# Initialize integrator states.
integrator_xc = (
diagram.GetMutableSubsystemState(integrator, context)
.get_mutable_continuous_state().get_vector())
integrator_xc.SetFromVector([0, 1, 2])
simulator.Initialize()
# Simulate briefly, and take full-context snapshots at intermediate
# points.
n = 6
times = np.linspace(0, 1, n)
context_log = []
for t in times:
simulator.AdvanceTo(t)
# Record snapshot of *entire* context.
context_log.append(context.Clone())
# Test binding for PrintSimulatorStatistics
PrintSimulatorStatistics(simulator)
xc_initial = np.array([0, 1, 2])
xc_final = np.array([0.123, 1.234, 2.345])
for i, context_i in enumerate(context_log):
t = times[i]
self.assertEqual(context_i.get_time(), t)
xc = context_i.get_continuous_state_vector().CopyToVector()
xc_expected = (float(i) / (n - 1) * (xc_final - xc_initial)
+ xc_initial)
self.assertTrue(np.allclose(xc, xc_expected))
class BabySystem(LeafSystem):
def __init__(self):
LeafSystem.__init__(self)
self.DeclareVectorInputPort("the_ins", BasicVector(1))
self.DeclareVectorOutputPort("the_outs", BasicVector(1),
self.CalcOutput)
def CalcOutput(self, context, output):
# print(output)
output.SetFromVector(np.array([0.1]))
builder = DiagramBuilder()
controller = builder.AddSystem(BabySystem())
integrator = builder.AddSystem(Integrator(1))
builder.Connect(controller.get_output_port(), integrator.get_input_port())
# builder.Connect(integrator)
diagram = builder.Build()
context = diagram.CreateDefaultContext()
simulator = Simulator(diagram)
# integrator.GetMyContextFromRoot(simulator.get_mutable_context()).get_mutable_continuous_state_vector().SetFromVector(np.array([2.3]))
g = Source(diagram.GetGraphvizString())
g.view()
# print(context)
int_context = integrator.GetMyContextFromRoot(simulator.get_mutable_context())
print(int_context)
