Python Simulator.set_target_realtime_rate方法代码示例

# 需要导入模块: from pydrake.systems.analysis import Simulator [as 别名]
# 或者: from pydrake.systems.analysis.Simulator import set_target_realtime_rate [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 set_target_realtime_rate [as 别名]
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)

# 需要导入模块: from pydrake.systems.analysis import Simulator [as 别名]
# 或者: from pydrake.systems.analysis.Simulator import set_target_realtime_rate [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 set_target_realtime_rate [as 别名]
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)

# 需要导入模块: from pydrake.systems.analysis import Simulator [as 别名]
# 或者: from pydrake.systems.analysis.Simulator import set_target_realtime_rate [as 别名]
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)
simulator.StepTo(args.duration)

# 需要导入模块: from pydrake.systems.analysis import Simulator [as 别名]
# 或者: from pydrake.systems.analysis.Simulator import set_target_realtime_rate [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 set_target_realtime_rate [as 别名]
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(
        "--duration", type=float, default=np.inf,
        help="Desired duration of the simulation in seconds.")
    parser.add_argument(
        "--hardware", action='store_true',
        help="Use the ManipulationStationHardwareInterface instead of an "
             "in-process simulation.")
    parser.add_argument(
        "--test", action='store_true',
        help="Disable opening the gui window for testing.")
    parser.add_argument(
        "--filter_time_const", type=float, default=0.005,
        help="Time constant for the first order low pass filter applied to"
             "the teleop commands")
    parser.add_argument(
        "--velocity_limit_factor", type=float, default=1.0,
        help="This value, typically between 0 and 1, further limits the "
             "iiwa14 joint velocities. It multiplies each of the seven "
             "pre-defined joint velocity limits. "
             "Note: The pre-defined velocity limits are specified by "
             "iiwa14_velocity_limits, found in this python file.")
    parser.add_argument(
        '--setup', type=str, default='default',
        help="The manipulation station setup to simulate. ",
        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"))
#.........这里部分代码省略.........

# 需要导入模块: from pydrake.systems.analysis import Simulator [as 别名]
# 或者: from pydrake.systems.analysis.Simulator import set_target_realtime_rate [as 别名]
 def make_simulator(generator):
     simulator = Simulator(system)
     simulator.Initialize()
     simulator.set_target_realtime_rate(0)
     return simulator