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)