Python Simulator.get_mutable_context方法代码示例

# 需要导入模块: from pydrake.systems.analysis import Simulator [as 别名]
# 或者: from pydrake.systems.analysis.Simulator import get_mutable_context [as 别名]
    def test_simulator_integrator_manipulation(self):
        system = ConstantVectorSource([1])

        # Create simulator with basic constructor.
        simulator = Simulator(system)
        simulator.Initialize()
        simulator.set_target_realtime_rate(0)

        integrator = simulator.get_mutable_integrator()

        target_accuracy = 1E-6
        integrator.set_target_accuracy(target_accuracy)
        self.assertEqual(integrator.get_target_accuracy(), target_accuracy)

        maximum_step_size = 0.2
        integrator.set_maximum_step_size(maximum_step_size)
        self.assertEqual(integrator.get_maximum_step_size(), maximum_step_size)

        minimum_step_size = 2E-2
        integrator.set_requested_minimum_step_size(minimum_step_size)
        self.assertEqual(integrator.get_requested_minimum_step_size(),
                         minimum_step_size)

        integrator.set_throw_on_minimum_step_size_violation(True)
        self.assertTrue(integrator.get_throw_on_minimum_step_size_violation())

        integrator.set_fixed_step_mode(True)
        self.assertTrue(integrator.get_fixed_step_mode())

        const_integrator = simulator.get_integrator()
        self.assertTrue(const_integrator is integrator)

        # Test context-less constructors for
        # integrator types.
        test_integrator = RungeKutta2Integrator(
            system=system, max_step_size=0.01)
        test_integrator = RungeKutta3Integrator(system=system)

        # Test simulator's reset_integrator,
        # and also the full constructors for
        # all integrator types.
        simulator.reset_integrator(
            RungeKutta2Integrator(
                system=system,
                max_step_size=0.01,
                context=simulator.get_mutable_context()))

        simulator.reset_integrator(
            RungeKutta3Integrator(
                system=system,
                context=simulator.get_mutable_context()))

# 需要导入模块: from pydrake.systems.analysis import Simulator [as 别名]
# 或者: from pydrake.systems.analysis.Simulator import get_mutable_context [as 别名]
    def test_simulator_ctor(self):
        # Create simple system.
        system = ConstantVectorSource([1])

        def check_output(context):
            # Check number of output ports and value for a given context.
            output = system.AllocateOutput(context)
            self.assertEquals(output.get_num_ports(), 1)
            system.CalcOutput(context, output)
            value = output.get_vector_data(0).get_value()
            self.assertTrue(np.allclose([1], value))

        # Create simulator with basic constructor.
        simulator = Simulator(system)
        simulator.Initialize()
        simulator.set_target_realtime_rate(0)
        simulator.set_publish_every_time_step(True)
        self.assertTrue(simulator.get_context() is
                        simulator.get_mutable_context())
        check_output(simulator.get_context())
        simulator.StepTo(1)

        # Create simulator specifying context.
        context = system.CreateDefaultContext()
        # @note `simulator` now owns `context`.
        simulator = Simulator(system, context)
        self.assertTrue(simulator.get_context() is context)
        check_output(context)
        simulator.StepTo(1)

# 需要导入模块: from pydrake.systems.analysis import Simulator [as 别名]
# 或者: from pydrake.systems.analysis.Simulator import get_mutable_context [as 别名]
    def test_simulation(self):
        # Basic constant-torque acrobot simulation.
        acrobot = AcrobotPlant()

        # Create the simulator.
        simulator = Simulator(acrobot)
        context = simulator.get_mutable_context()

        # Set an input torque.
        input = AcrobotInput()
        input.set_tau(1.)
        context.FixInputPort(0, input)

        # Set the initial state.
        state = context.get_mutable_continuous_state_vector()
        state.set_theta1(1.)
        state.set_theta1dot(0.)
        state.set_theta2(0.)
        state.set_theta2dot(0.)

        self.assertTrue(acrobot.DynamicsBiasTerm(context).shape == (2,))
        self.assertTrue(acrobot.MassMatrix(context).shape == (2, 2))
        initial_total_energy = acrobot.CalcPotentialEnergy(context) + \
            acrobot.CalcKineticEnergy(context)

        # Simulate (and make sure the state actually changes).
        initial_state = state.CopyToVector()
        simulator.StepTo(1.0)

        self.assertLessEqual(acrobot.CalcPotentialEnergy(context) +
                             acrobot.CalcKineticEnergy(context),
                             initial_total_energy)

# 需要导入模块: from pydrake.systems.analysis import Simulator [as 别名]
# 或者: from pydrake.systems.analysis.Simulator import get_mutable_context [as 别名]
    def test_simulation(self):
        van_der_pol = VanDerPolOscillator()

        # Create the simulator.
        simulator = Simulator(van_der_pol)
        context = simulator.get_mutable_context()

        # Set the initial state.
        state = context.get_mutable_continuous_state_vector()
        state.SetFromVector([0., 2.])

        # Simulate (and make sure the state actually changes).
        initial_state = state.CopyToVector()
        simulator.StepTo(1.0)
        self.assertFalse((state.CopyToVector() == initial_state).any())

# 需要导入模块: from pydrake.systems.analysis import Simulator [as 别名]
# 或者: from pydrake.systems.analysis.Simulator import get_mutable_context [as 别名]
    def test_simulation(self):
        # Basic rimless_wheel simulation.
        rimless_wheel = RimlessWheel()

        # Create the simulator.
        simulator = Simulator(rimless_wheel)
        context = simulator.get_mutable_context()
        context.set_accuracy(1e-8)

        # Set the initial state.
        state = context.get_mutable_continuous_state_vector()
        state.set_theta(0.)
        state.set_thetadot(4.)

        # Simulate (and make sure the state actually changes).
        initial_state = state.CopyToVector()
        simulator.StepTo(1.0)
        self.assertFalse((state.CopyToVector() == initial_state).any())

# 需要导入模块: from pydrake.systems.analysis import Simulator [as 别名]
# 或者: from pydrake.systems.analysis.Simulator import get_mutable_context [as 别名]
    def test_simulation(self):
        # Basic compass_gait simulation.
        compass_gait = CompassGait()

        # Create the simulator.
        simulator = Simulator(compass_gait)
        context = simulator.get_mutable_context()
        context.SetAccuracy(1e-8)

        # Set the initial state.
        state = context.get_mutable_continuous_state_vector()
        state.set_stance(0.)
        state.set_swing(0.)
        state.set_stancedot(0.4)
        state.set_swingdot(-2.0)

        # Simulate (and make sure the state actually changes).
        initial_state = state.CopyToVector()
        simulator.AdvanceTo(1.0)
        self.assertFalse((state.CopyToVector() == initial_state).any())

# 需要导入模块: from pydrake.systems.analysis import Simulator [as 别名]
# 或者: from pydrake.systems.analysis.Simulator import get_mutable_context [as 别名]
    def test_simulation(self):
        # Basic constant-torque pendulum simulation.
        pendulum = PendulumPlant()

        # Create the simulator.
        simulator = Simulator(pendulum)
        context = simulator.get_mutable_context()

        # Set an input torque.
        input = PendulumInput()
        input.set_tau(1.)
        context.FixInputPort(0, input)

        # Set the initial state.
        state = context.get_mutable_continuous_state_vector()
        state.set_theta(1.)
        state.set_thetadot(0.)

        # Simulate (and make sure the state actually changes).
        initial_state = state.CopyToVector()
        simulator.StepTo(1.0)
        self.assertFalse((state.CopyToVector() == initial_state).any())

# 需要导入模块: from pydrake.systems.analysis import Simulator [as 别名]
# 或者: from pydrake.systems.analysis.Simulator import get_mutable_context [as 别名]
    def test_simple_car(self):
        simple_car = SimpleCar()
        simulator = Simulator(simple_car)
        context = simulator.get_mutable_context()
        output = simple_car.AllocateOutput()

        # Fix the input.
        command = DrivingCommand()
        command.set_steering_angle(0.5)
        command.set_acceleration(1.)
        context.FixInputPort(0, command)

        # Verify the inputs.
        command_eval = simple_car.EvalVectorInput(context, 0)
        self.assertTrue(np.allclose(
            command.get_value(), command_eval.get_value()))

        # Initialize all the states to zero and take a simulation step.
        state = context.get_mutable_continuous_state_vector()
        state.SetFromVector([0.] * state.size())
        simulator.StepTo(1.0)

        # Verify the outputs.
        simple_car.CalcOutput(context, output)
        state_index = simple_car.state_output().get_index()
        state_value = output.get_vector_data(state_index)
        self.assertIsInstance(state_value, SimpleCarState)
        self.assertTrue(
            np.allclose(state.CopyToVector(), state_value.get_value()))
        pose_index = simple_car.pose_output().get_index()
        pose_value = output.get_vector_data(pose_index)
        self.assertIsInstance(pose_value, PoseVector)
        self.assertTrue(pose_value.get_translation()[0] > 0.)
        velocity_index = simple_car.velocity_output().get_index()
        velocity_value = output.get_vector_data(velocity_index)
        self.assertIsInstance(velocity_value, FrameVelocity)
        self.assertTrue(velocity_value.get_velocity().translational()[0] > 0.)

# 需要导入模块: from pydrake.systems.analysis import Simulator [as 别名]
# 或者: from pydrake.systems.analysis.Simulator import get_mutable_context [as 别名]
                                                   size=7))
builder.Connect(teleop.get_output_port(0), filter.get_input_port(0))
builder.Connect(filter.get_output_port(0),
                station.GetInputPort("iiwa_position"))

wsg_buttons = builder.AddSystem(SchunkWsgButtons(teleop.window))
builder.Connect(wsg_buttons.GetOutputPort("position"), station.GetInputPort(
    "wsg_position"))
builder.Connect(wsg_buttons.GetOutputPort("force_limit"),
                station.GetInputPort("wsg_force_limit"))

diagram = builder.Build()
simulator = Simulator(diagram)

station_context = diagram.GetMutableSubsystemContext(
    station, simulator.get_mutable_context())

station_context.FixInputPort(station.GetInputPort(
    "iiwa_feedforward_torque").get_index(), np.zeros(7))

# Eval the output port once to read the initial positions of the IIWA.
q0 = station.GetOutputPort("iiwa_position_measured").Eval(
    station_context).get_value()
teleop.set_position(q0)
filter.set_initial_output_value(diagram.GetMutableSubsystemContext(
    filter, simulator.get_mutable_context()), q0)

# This is important to avoid duplicate publishes to the hardware interface:
simulator.set_publish_every_time_step(False)

simulator.set_target_realtime_rate(args.target_realtime_rate)

# 需要导入模块: from pydrake.systems.analysis import Simulator [as 别名]
# 或者: from pydrake.systems.analysis.Simulator import get_mutable_context [as 别名]

#.........这里部分代码省略.........

            def DoCalcTimeDerivatives(self, context, derivatives):
                # Note:  Don't call base method here; it would abort because
                # derivatives.size() != 0.
                test.assertEqual(derivatives.get_vector().size(), 2)
                self.called_continuous = True

            def DoCalcDiscreteVariableUpdates(
                    self, context, events, discrete_state):
                # Call base method to ensure we do not get recursion.
                LeafSystem.DoCalcDiscreteVariableUpdates(
                    self, context, events, discrete_state)
                self.called_discrete = True

            def DoGetWitnessFunctions(self, context):
                self.called_getwitness = True
                return [self.witness, self.reset_witness]

            def _on_initialize(self, context, event):
                test.assertIsInstance(context, Context)
                test.assertIsInstance(event, PublishEvent)
                test.assertFalse(self.called_initialize)
                self.called_initialize = True

            def _on_per_step(self, context, event):
                test.assertIsInstance(context, Context)
                test.assertIsInstance(event, PublishEvent)
                self.called_per_step = True

            def _on_periodic(self, context, event):
                test.assertIsInstance(context, Context)
                test.assertIsInstance(event, PublishEvent)
                test.assertFalse(self.called_periodic)
                self.called_periodic = True

            def _witness(self, context):
                test.assertIsInstance(context, Context)
                self.called_witness = True
                return 1.0

            def _guard(self, context):
                test.assertIsInstance(context, Context)
                self.called_guard = True
                return context.get_time() - 0.5

            def _reset(self, context, event, state):
                test.assertIsInstance(context, Context)
                test.assertIsInstance(event, UnrestrictedUpdateEvent)
                test.assertIsInstance(state, State)
                self.called_reset = True

        system = TrivialSystem()
        self.assertFalse(system.called_publish)
        self.assertFalse(system.called_feedthrough)
        self.assertFalse(system.called_continuous)
        self.assertFalse(system.called_discrete)
        self.assertFalse(system.called_initialize)
        results = call_leaf_system_overrides(system)
        self.assertTrue(system.called_publish)
        self.assertTrue(system.called_feedthrough)
        self.assertFalse(results["has_direct_feedthrough"])
        self.assertTrue(system.called_continuous)
        self.assertTrue(system.called_discrete)
        self.assertTrue(system.called_initialize)
        self.assertEqual(results["discrete_next_t"], 1.0)

        self.assertFalse(system.HasAnyDirectFeedthrough())
        self.assertFalse(system.HasDirectFeedthrough(output_port=0))
        self.assertFalse(
            system.HasDirectFeedthrough(input_port=0, output_port=0))

        # Test explicit calls.
        system = TrivialSystem()
        context = system.CreateDefaultContext()
        system.Publish(context)
        self.assertTrue(system.called_publish)
        context_update = context.Clone()
        system.CalcTimeDerivatives(
            context=context,
            derivatives=context_update.get_mutable_continuous_state())
        self.assertTrue(system.called_continuous)
        witnesses = system.GetWitnessFunctions(context)
        self.assertEqual(len(witnesses), 2)
        system.CalcDiscreteVariableUpdates(
            context=context,
            discrete_state=context_update.get_mutable_discrete_state())
        self.assertTrue(system.called_discrete)

        # Test per-step, periodic, and witness call backs
        system = TrivialSystem()
        simulator = Simulator(system)
        simulator.get_mutable_context().SetAccuracy(0.1)
        # Stepping to 0.99 so that we get exactly one periodic event.
        simulator.AdvanceTo(0.99)
        self.assertTrue(system.called_per_step)
        self.assertTrue(system.called_periodic)
        self.assertTrue(system.called_getwitness)
        self.assertTrue(system.called_witness)
        self.assertTrue(system.called_guard)
        self.assertTrue(system.called_reset)

# 需要导入模块: from pydrake.systems.analysis import Simulator [as 别名]
# 或者: from pydrake.systems.analysis.Simulator import get_mutable_context [as 别名]
    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 = BasicVector([0.1, 0.2, 0.3])
        context.FixInputPort(0, input0)
        input1 = BasicVector([0.02, 0.03, 0.04])
        context.FixInputPort(1, input1)
        input2 = BasicVector([0.003, 0.004, 0.005])
        context.FixInputPort(2, 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.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))

# 需要导入模块: from pydrake.systems.analysis import Simulator [as 别名]
# 或者: from pydrake.systems.analysis.Simulator import get_mutable_context [as 别名]
builder = DiagramBuilder()

plant = builder.AddSystem(QuadrotorPlant())

controller = builder.AddSystem(StabilizingLQRController(plant, [0, 0, 1]))
builder.Connect(controller.get_output_port(0), plant.get_input_port(0))
builder.Connect(plant.get_output_port(0), controller.get_input_port(0))

# Set up visualization in MeshCat
scene_graph = builder.AddSystem(SceneGraph())
QuadrotorGeometry.AddToBuilder(builder, plant.get_output_port(0), scene_graph)
meshcat = builder.AddSystem(MeshcatVisualizer(
    scene_graph, zmq_url=args.meshcat,
    open_browser=args.open_browser))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
                meshcat.get_input_port(0))
# end setup for visualization

diagram = builder.Build()

simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
context = simulator.get_mutable_context()

for i in range(args.trials):
    context.SetTime(0.)
    context.SetContinuousState(np.random.randn(12,))
    simulator.Initialize()
    simulator.StepTo(args.duration)

# 需要导入模块: from pydrake.systems.analysis import Simulator [as 别名]
# 或者: from pydrake.systems.analysis.Simulator import get_mutable_context [as 别名]
    FirstOrderLowPassFilter(time_constant=args.filter_time_const, size=6))

builder.Connect(teleop.get_output_port(0), filter.get_input_port(0))
builder.Connect(filter.get_output_port(0),
                differential_ik.GetInputPort("rpy_xyz_desired"))

builder.Connect(teleop.GetOutputPort("position"), station.GetInputPort(
    "wsg_position"))
builder.Connect(teleop.GetOutputPort("force_limit"),
                station.GetInputPort("wsg_force_limit"))

diagram = builder.Build()
simulator = Simulator(diagram)

station_context = diagram.GetMutableSubsystemContext(
    station, simulator.get_mutable_context())

station_context.FixInputPort(station.GetInputPort(
    "iiwa_feedforward_torque").get_index(), np.zeros(7))

q0 = station.GetOutputPort("iiwa_position_measured").Eval(station_context)
differential_ik.parameters.set_nominal_joint_position(q0)

teleop.SetPose(differential_ik.ForwardKinematics(q0))
filter.set_initial_output_value(
    diagram.GetMutableSubsystemContext(
        filter, simulator.get_mutable_context()),
    teleop.get_output_port(0).Eval(diagram.GetMutableSubsystemContext(
        teleop, simulator.get_mutable_context())))
differential_ik.SetPositions(diagram.GetMutableSubsystemContext(
    differential_ik, simulator.get_mutable_context()), q0)

# 需要导入模块: from pydrake.systems.analysis import Simulator [as 别名]
# 或者: from pydrake.systems.analysis.Simulator import get_mutable_context [as 别名]

#.........这里部分代码省略.........
        choices=['default', 'clutter_clearing'])
    MeshcatVisualizer.add_argparse_argument(parser)
    args = parser.parse_args()

    if args.test:
        # Don't grab mouse focus during testing.
        grab_focus = False
        # See: https://stackoverflow.com/a/52528832/7829525
        os.environ["SDL_VIDEODRIVER"] = "dummy"
    else:
        grab_focus = True

    builder = DiagramBuilder()

    if args.hardware:
        station = builder.AddSystem(ManipulationStationHardwareInterface())
        station.Connect(wait_for_cameras=False)
    else:
        station = builder.AddSystem(ManipulationStation())

        # Initializes the chosen station type.
        if args.setup == 'default':
            station.SetupDefaultStation()
        elif args.setup == 'clutter_clearing':
            station.SetupClutterClearingStation()
            ycb_objects = CreateDefaultYcbObjectList()
            for model_file, X_WObject in ycb_objects:
                station.AddManipulandFromFile(model_file, X_WObject)

        station.Finalize()
        ConnectDrakeVisualizer(builder, station.get_scene_graph(),
                               station.GetOutputPort("pose_bundle"))
        if args.meshcat:
            meshcat = builder.AddSystem(MeshcatVisualizer(
                station.get_scene_graph(), zmq_url=args.meshcat))
            builder.Connect(station.GetOutputPort("pose_bundle"),
                            meshcat.get_input_port(0))

    robot = station.get_controller_plant()
    params = DifferentialInverseKinematicsParameters(robot.num_positions(),
                                                     robot.num_velocities())

    time_step = 0.005
    params.set_timestep(time_step)
    # True velocity limits for the IIWA14 (in rad, rounded down to the first
    # decimal)
    iiwa14_velocity_limits = np.array([1.4, 1.4, 1.7, 1.3, 2.2, 2.3, 2.3])
    # Stay within a small fraction of those limits for this teleop demo.
    factor = args.velocity_limit_factor
    params.set_joint_velocity_limits((-factor*iiwa14_velocity_limits,
                                      factor*iiwa14_velocity_limits))

    differential_ik = builder.AddSystem(DifferentialIK(
        robot, robot.GetFrameByName("iiwa_link_7"), params, time_step))

    builder.Connect(differential_ik.GetOutputPort("joint_position_desired"),
                    station.GetInputPort("iiwa_position"))

    teleop = builder.AddSystem(MouseKeyboardTeleop(grab_focus=grab_focus))
    filter_ = builder.AddSystem(
        FirstOrderLowPassFilter(time_constant=args.filter_time_const, size=6))

    builder.Connect(teleop.get_output_port(0), filter_.get_input_port(0))
    builder.Connect(filter_.get_output_port(0),
                    differential_ik.GetInputPort("rpy_xyz_desired"))

    builder.Connect(teleop.GetOutputPort("position"), station.GetInputPort(
        "wsg_position"))
    builder.Connect(teleop.GetOutputPort("force_limit"),
                    station.GetInputPort("wsg_force_limit"))

    diagram = builder.Build()
    simulator = Simulator(diagram)

    # This is important to avoid duplicate publishes to the hardware interface:
    simulator.set_publish_every_time_step(False)

    station_context = diagram.GetMutableSubsystemContext(
        station, simulator.get_mutable_context())

    station.GetInputPort("iiwa_feedforward_torque").FixValue(
        station_context, np.zeros(7))

    simulator.AdvanceTo(1e-6)
    q0 = station.GetOutputPort("iiwa_position_measured").Eval(station_context)
    differential_ik.parameters.set_nominal_joint_position(q0)

    teleop.SetPose(differential_ik.ForwardKinematics(q0))
    filter_.set_initial_output_value(
        diagram.GetMutableSubsystemContext(
            filter_, simulator.get_mutable_context()),
        teleop.get_output_port(0).Eval(diagram.GetMutableSubsystemContext(
            teleop, simulator.get_mutable_context())))
    differential_ik.SetPositions(diagram.GetMutableSubsystemContext(
        differential_ik, simulator.get_mutable_context()), q0)

    simulator.set_target_realtime_rate(args.target_realtime_rate)

    print_instructions()
    simulator.AdvanceTo(args.duration)

# 需要导入模块: from pydrake.systems.analysis import Simulator [as 别名]
# 或者: from pydrake.systems.analysis.Simulator import get_mutable_context [as 别名]
                                          state_grid, input_grid,
                                          timestep, options)

J = np.reshape(cost_to_go, Q.shape)
surf = ax.plot_surface(Q, Qdot, J, rstride=1, cstride=1,
                       cmap=cm.jet)
Pi = np.reshape(policy.get_output_values(), Q.shape)
surf = ax2.plot_surface(Q, Qdot, Pi, rstride=1, cstride=1,
                        cmap=cm.jet)

# animate the resulting policy.
builder = DiagramBuilder()
plant = builder.AddSystem(DoubleIntegrator())
logger = LogOutput(plant.get_output_port(0), builder)
vi_policy = builder.AddSystem(policy)
builder.Connect(vi_policy.get_output_port(0), plant.get_input_port(0))
builder.Connect(plant.get_output_port(0), vi_policy.get_input_port(0))

diagram = builder.Build()
simulator = Simulator(diagram)

state = simulator.get_mutable_context().SetContinuousState([-10.0, 0.0])

simulator.StepTo(10.)

# Visualize the result as a video.
vis = DoubleIntegratorVisualizer()
ani = vis.animate(logger, repeat=True)

plt.show()