Python analysis.Simulator类代码示例

    def test_texture_override(self):
        """Draws a textured box to test the texture override pathway."""
        object_file_path = FindResourceOrThrow(
            "drake/systems/sensors/test/models/box_with_mesh.sdf")
        # Find the texture path just to ensure it exists and
        # we're testing the code path we want to.
        FindResourceOrThrow("drake/systems/sensors/test/models/meshes/box.png")

        builder = DiagramBuilder()
        plant = MultibodyPlant(0.002)
        _, scene_graph = AddMultibodyPlantSceneGraph(builder, plant)
        object_model = Parser(plant=plant).AddModelFromFile(object_file_path)
        plant.Finalize()

        # Add meshcat visualizer.
        viz = builder.AddSystem(
            MeshcatVisualizer(scene_graph,
                              zmq_url=None,
                              open_browser=False))
        builder.Connect(
            scene_graph.get_pose_bundle_output_port(),
            viz.get_input_port(0))

        diagram = builder.Build()
        diagram_context = diagram.CreateDefaultContext()

        simulator = Simulator(diagram, diagram_context)
        simulator.set_publish_every_time_step(False)
        simulator.AdvanceTo(1.0)

    def test_kuka(self):
        """Kuka IIWA with mesh geometry."""
        file_name = FindResourceOrThrow(
            "drake/manipulation/models/iiwa_description/sdf/"
            "iiwa14_no_collision.sdf")
        builder = DiagramBuilder()
        kuka, scene_graph = AddMultibodyPlantSceneGraph(builder)
        Parser(plant=kuka).AddModelFromFile(file_name)
        kuka.Finalize()

        visualizer = builder.AddSystem(MeshcatVisualizer(scene_graph,
                                                         zmq_url=ZMQ_URL,
                                                         open_browser=False))
        builder.Connect(scene_graph.get_pose_bundle_output_port(),
                        visualizer.get_input_port(0))

        diagram = builder.Build()

        diagram_context = diagram.CreateDefaultContext()
        kuka_context = diagram.GetMutableSubsystemContext(
            kuka, diagram_context)

        kuka_context.FixInputPort(
            kuka.get_actuation_input_port().get_index(), np.zeros(
                kuka.get_actuation_input_port().size()))

        simulator = Simulator(diagram, diagram_context)
        simulator.set_publish_every_time_step(False)
        simulator.AdvanceTo(.1)

    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)

    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)

 def show_cloud(pc, use_native=False, **kwargs):
     # kwargs go to ctor.
     builder = DiagramBuilder()
     # Add point cloud visualization.
     if use_native:
         viz = meshcat.Visualizer(zmq_url=ZMQ_URL)
     else:
         plant, scene_graph = AddMultibodyPlantSceneGraph(builder)
         plant.Finalize()
         viz = builder.AddSystem(MeshcatVisualizer(
             scene_graph, zmq_url=ZMQ_URL, open_browser=False))
         builder.Connect(
             scene_graph.get_pose_bundle_output_port(),
             viz.get_input_port(0))
     pc_viz = builder.AddSystem(
         MeshcatPointCloudVisualizer(viz, **kwargs))
     # Make sure the system runs.
     diagram = builder.Build()
     diagram_context = diagram.CreateDefaultContext()
     context = diagram.GetMutableSubsystemContext(
         pc_viz, diagram_context)
     context.FixInputPort(
         pc_viz.GetInputPort("point_cloud_P").get_index(),
         AbstractValue.Make(pc))
     simulator = Simulator(diagram, diagram_context)
     simulator.set_publish_every_time_step(False)
     simulator.AdvanceTo(sim_time)

    def test_cart_pole(self):
        """Cart-Pole with simple geometry."""
        file_name = FindResourceOrThrow(
            "drake/examples/multibody/cart_pole/cart_pole.sdf")

        builder = DiagramBuilder()
        cart_pole, scene_graph = AddMultibodyPlantSceneGraph(builder)
        Parser(plant=cart_pole).AddModelFromFile(file_name)
        cart_pole.Finalize()
        assert cart_pole.geometry_source_is_registered()

        visualizer = builder.AddSystem(MeshcatVisualizer(scene_graph,
                                                         zmq_url=ZMQ_URL,
                                                         open_browser=False))
        builder.Connect(scene_graph.get_pose_bundle_output_port(),
                        visualizer.get_input_port(0))

        diagram = builder.Build()

        diagram_context = diagram.CreateDefaultContext()
        cart_pole_context = diagram.GetMutableSubsystemContext(
            cart_pole, diagram_context)

        cart_pole_context.FixInputPort(
            cart_pole.get_actuation_input_port().get_index(), [0])

        cart_slider = cart_pole.GetJointByName("CartSlider")
        pole_pin = cart_pole.GetJointByName("PolePin")
        cart_slider.set_translation(context=cart_pole_context, translation=0.)
        pole_pin.set_angle(context=cart_pole_context, angle=2.)

        simulator = Simulator(diagram, diagram_context)
        simulator.set_publish_every_time_step(False)
        simulator.AdvanceTo(.1)

def main():
    parser = argparse.ArgumentParser(description=__doc__)
    parser.add_argument(
        "--target_realtime_rate", type=float, default=1.0,
        help="Desired rate relative to real time.  See documentation for "
             "Simulator::set_target_realtime_rate() for details.")
    parser.add_argument(
        "--simulation_time", type=float, default=10.0,
        help="Desired duration of the simulation in seconds.")
    parser.add_argument(
        "--time_step", type=float, default=0.,
        help="If greater than zero, the plant is modeled as a system with "
             "discrete updates and period equal to this time_step. "
             "If 0, the plant is modeled as a continuous system.")
    args = parser.parse_args()

    file_name = FindResourceOrThrow(
        "drake/examples/multibody/cart_pole/cart_pole.sdf")
    builder = DiagramBuilder()
    scene_graph = builder.AddSystem(SceneGraph())
    cart_pole = builder.AddSystem(MultibodyPlant(time_step=args.time_step))
    cart_pole.RegisterAsSourceForSceneGraph(scene_graph)
    Parser(plant=cart_pole).AddModelFromFile(file_name)
    cart_pole.AddForceElement(UniformGravityFieldElement())
    cart_pole.Finalize()
    assert cart_pole.geometry_source_is_registered()

    builder.Connect(
        scene_graph.get_query_output_port(),
        cart_pole.get_geometry_query_input_port())
    builder.Connect(
        cart_pole.get_geometry_poses_output_port(),
        scene_graph.get_source_pose_port(cart_pole.get_source_id()))

    ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph)
    diagram = builder.Build()

    diagram_context = diagram.CreateDefaultContext()
    cart_pole_context = diagram.GetMutableSubsystemContext(
        cart_pole, diagram_context)

    cart_pole_context.FixInputPort(
        cart_pole.get_actuation_input_port().get_index(), [0])

    cart_slider = cart_pole.GetJointByName("CartSlider")
    pole_pin = cart_pole.GetJointByName("PolePin")
    cart_slider.set_translation(context=cart_pole_context, translation=0.)
    pole_pin.set_angle(context=cart_pole_context, angle=2.)

    simulator = Simulator(diagram, diagram_context)
    simulator.set_publish_every_time_step(False)
    simulator.set_target_realtime_rate(args.target_realtime_rate)
    simulator.Initialize()
    simulator.AdvanceTo(args.simulation_time)

    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())

    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())

    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())

    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())

    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

from plant import SLIPState, SpringLoadedInvertedPendulum

plant = SpringLoadedInvertedPendulum()

# Parameters from Geyer05, Figure 2.4
# Note: Geyer uses angle of attack = 90 - touchdown_angle
touchdown_angle = np.deg2rad(30)
Etilde = 1.61

s = SLIPState(np.zeros(8))
s.theta = touchdown_angle
s.r = 1

simulator = Simulator(plant)
context = simulator.get_mutable_context()
context.FixInputPort(0, [touchdown_angle])
context.SetAccuracy(1e-5)

zs = np.linspace(np.cos(touchdown_angle)+0.001, 0.95, 25)
zns = 0*zs
for i in range(len(zs)):
    s.z = zs[i]
    s.xdot = plant.apex_velocity_from_dimensionless_system_energy(Etilde, s.z)
    context.SetTime(0.)
    context.get_mutable_continuous_state_vector().SetFromVector(s[:])
    simulator.Initialize()
    # Note: With this duration, I sometimes get an extra "touchdown" after the
    # apex, which results in apex-touchdown; touchdown-takeoff-apex on the
    # console.  It's not a double reset, the consecutive touchdowns are two

def main():
    args_parser = argparse.ArgumentParser(
        description=__doc__,
        formatter_class=argparse.RawDescriptionHelpFormatter)
    args_parser.add_argument(
        "filename", type=str,
        help="Path to an SDF or URDF file.")
    args_parser.add_argument(
        "--package_path",
        type=str,
        default=None,
        help="Full path to the root package for reading in SDF resources.")
    position_group = args_parser.add_mutually_exclusive_group()
    position_group.add_argument(
        "--position", type=float, nargs="+", default=[],
        help="A list of positions which must be the same length as the number "
             "of positions in the sdf model.  Note that most models have a "
             "floating-base joint by default (unless the sdf explicitly welds "
             "the base to the world, and so have 7 positions corresponding to "
             "the quaternion representation of that floating-base position.")
    position_group.add_argument(
        "--joint_position", type=float, nargs="+", default=[],
        help="A list of positions which must be the same length as the number "
             "of positions ASSOCIATED WITH JOINTS in the sdf model.  This "
             "does not include, e.g., floating-base coordinates, which will "
             "be assigned a default value.")
    args_parser.add_argument(
        "--test", action='store_true',
        help="Disable opening the gui window for testing.")
    # TODO(russt): Add option to weld the base to the world pending the
    # availability of GetUniqueBaseBody requested in #9747.
    args = args_parser.parse_args()
    filename = args.filename
    if not os.path.isfile(filename):
        args_parser.error("File does not exist: {}".format(filename))

    builder = DiagramBuilder()
    scene_graph = builder.AddSystem(SceneGraph())

    # Construct a MultibodyPlant.
    plant = MultibodyPlant()
    plant.RegisterAsSourceForSceneGraph(scene_graph)

    # Create the parser.
    parser = Parser(plant)

    # Get the package pathname.
    if args.package_path:
        # Verify that package.xml is found in the designated path.
        package_path = os.path.abspath(args.package_path)
        if not os.path.isfile(os.path.join(package_path, "package.xml")):
            parser.error("package.xml not found at: {}".format(package_path))

        # Get the package map and populate it using the package path.
        package_map = parser.package_map()
        package_map.PopulateFromFolder(package_path)

    # Add the model from the file and finalize the plant.
    parser.AddModelFromFile(filename)
    plant.Finalize(scene_graph)

    # Add sliders to set positions of the joints.
    sliders = builder.AddSystem(JointSliders(robot=plant))
    to_pose = builder.AddSystem(MultibodyPositionToGeometryPose(plant))
    builder.Connect(sliders.get_output_port(0), to_pose.get_input_port())
    builder.Connect(
        to_pose.get_output_port(),
        scene_graph.get_source_pose_port(plant.get_source_id()))

    # Connect this to drake_visualizer.
    ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph)

    if len(args.position):
        sliders.set_position(args.position)
    elif len(args.joint_position):
        sliders.set_joint_position(args.joint_position)

    # Make the diagram and run it.
    diagram = builder.Build()
    simulator = Simulator(diagram)

    simulator.set_publish_every_time_step(False)

    if args.test:
        sliders.window.withdraw()
        simulator.StepTo(0.1)
    else:
        simulator.set_target_realtime_rate(1.0)
        simulator.StepTo(np.inf)

    teleop.window.withdraw()  # Don't display the window when testing.

filter = builder.AddSystem(FirstOrderLowPassFilter(time_constant=2.0,
                                                   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: