Python framework.LeafSystem类代码示例

    def __init__(self, meshcat_viz, draw_period=_DEFAULT_PUBLISH_PERIOD,
                 name="point_cloud", X_WP=Isometry3.Identity(),
                 default_rgb=[255., 255., 255.]):
        """
        Args:
            meshcat_viz: Either a native meshcat.Visualizer or a pydrake
                MeshcatVisualizer object.
            draw_period: The rate at which this class publishes to the
                visualizer.
            name: The string name of the meshcat object.
            X_WP: Pose of point cloud frame ``P`` in meshcat world frame ``W``.
                Default is identity.
            default_rgb: RGB value for published points if the PointCloud does
                not provide RGB values.
        """
        LeafSystem.__init__(self)

        self._meshcat_viz = _get_native_visualizer(meshcat_viz)
        self._X_WP = X_WP
        self._default_rgb = np.array(default_rgb)
        self._name = name

        self.set_name('meshcat_point_cloud_visualizer')
        self._DeclarePeriodicPublish(draw_period, 0.0)

        self._DeclareAbstractInputPort("point_cloud_P",
                                       AbstractValue.Make(mut.PointCloud()))

    def __init__(self, robot, frame_E, parameters, time_step):
        """
        @param robot is a reference to a MultibodyPlant.
        @param frame_E is a multibody::Frame on the robot.
        @param params is a DifferentialIKParams.
        @params time_step This system updates its state/outputs at discrete
                          periodic intervals defined with period @p time_step.
        """
        LeafSystem.__init__(self)
        self.robot = robot
        self.frame_E = frame_E
        self.parameters = parameters
        self.parameters.set_timestep(time_step)
        self.time_step = time_step
        # Note that this context is NOT the context of the DifferentialIK
        # system, but rather a context for the multibody plant that is used
        # to pass the configuration into the DifferentialInverseKinematics
        # methods.
        self.robot_context = robot.CreateDefaultContext()
        # Confirm that all velocities are zero (they will not be reset below).
        assert not self.robot.GetPositionsAndVelocities(
            self.robot_context)[-robot.num_velocities():].any()

        # Store the robot positions as state.
        self.DeclareDiscreteState(robot.num_positions())
        self.DeclarePeriodicDiscreteUpdate(time_step)

        # Desired pose of frame E in world frame.
        self.DeclareInputPort("rpy_xyz_desired",
                              PortDataType.kVectorValued, 6)

        # Provide the output as desired positions.
        self.DeclareVectorOutputPort("joint_position_desired", BasicVector(
            robot.num_positions()), self.CopyPositionOut)

 def __init__(self):
     LeafSystem.__init__(self)
     self.DeclareContinuousState(1)
     self.DeclareDiscreteState(2)
     self.DeclareAbstractState(model_value.Clone())
     self.DeclareAbstractParameter(model_value.Clone())
     self.DeclareNumericParameter(model_vector.Clone())

    def __init__(self):
        LeafSystem.__init__(self)

        self.DeclareVectorInputPort("touchdown_angle", BasicVector(1))
        self.DeclareContinuousState(BasicVector(np.zeros(8)), 4, 4, 0)

        self.DeclareVectorOutputPort("state", BasicVector(8),
                                     self.CopyStateOut)

        # Parameters from Geyer05, p.23
        self.mass = 80.  # kg
        self.r0 = 1.  # m
        self.gravity = 9.81  # m/s^2
        # Define spring constant in terms of the dimensionless number.
        # Definition in section 2.4.3, values in figure 2.4.
        # Note: Geyer05 says 10.8 (which doesn't work? -- I get no fixed pts).
        dimensionless_spring_constant = 10.7
        self.stiffness = \
            dimensionless_spring_constant*self.mass*self.gravity/self.r0

        self.last_apex = None  # placeholder for writing return map result.

        self.touchdown_witness = self.MakeWitnessFunction(
            "touchdown", WitnessFunctionDirection.kPositiveThenNonPositive,
            self.foot_height, UnrestrictedUpdateEvent(self.touchdown))
        self.takeoff_witness = self.MakeWitnessFunction(
            "takeoff", WitnessFunctionDirection.kPositiveThenNonPositive,
            self.leg_compression, UnrestrictedUpdateEvent(self.takeoff))
        self.apex_witness = self.MakeWitnessFunction(
            "apex", WitnessFunctionDirection.kPositiveThenNonPositive,
            self.apex, PublishEvent(self.publish_apex))

 def _DoPublish(self, context, events):
     # Call base method to ensure we do not get recursion.
     LeafSystem._DoPublish(self, context, events)
     # N.B. We do not test for a singular call to `DoPublish`
     # (checking `assertFalse(self.called_publish)` first) because
     # the above `_DeclareInitializationEvent` will call both its
     # callback and this event when invoked via
     # `Simulator::Initialize` from `call_leaf_system_overrides`,
     # even when we explicitly say not to publish at initialize.
     self.called_publish = True

 def __init__(self):
     LeafSystem.__init__(self)
     self.called_publish = False
     self.called_feedthrough = False
     self.called_discrete = False
     # Ensure we have desired overloads.
     self._DeclarePeriodicPublish(0.1)
     self._DeclarePeriodicPublish(0.1, 0)
     self._DeclarePeriodicPublish(period_sec=0.1, offset_sec=0.)
     self._DeclarePeriodicDiscreteUpdate(
         period_sec=0.1, offset_sec=0.)
     self._DeclareDiscreteState(1)
     # Ensure that we have inputs / outputs to call direct
     # feedthrough.
     self._DeclareInputPort(PortDataType.kVectorValued, 1)
     self._DeclareVectorOutputPort(BasicVector(1), noop)

    def test_deprecated_protected_aliases(self):
        """Tests a subset of protected aliases, pursuant to #9651."""

        class OldSystem(LeafSystem):
            def __init__(self):
                LeafSystem.__init__(self)
                self.called_publish = False
                # Check a non-overridable method
                with catch_drake_warnings(expected_count=1):
                    self._DeclareVectorInputPort("x", BasicVector(1))

            def _DoPublish(self, context, events):
                self.called_publish = True

        # Ensure old overrides are still used
        system = OldSystem()
        context = system.CreateDefaultContext()
        with catch_drake_warnings(expected_count=1):
            system.Publish(context)
        self.assertTrue(system.called_publish)

        # Ensure documentation doesn't duplicate stuff.
        with catch_drake_warnings(expected_count=1):
            self.assertIn("deprecated", LeafSystem._DoPublish.__doc__)
        # This will warn both on (a) calling the method and (b) on the
        # invocation of the override.
        with catch_drake_warnings(expected_count=2):
            LeafSystem._DoPublish(system, context, [])

        class AccidentallyBothSystem(LeafSystem):
            def __init__(self):
                LeafSystem.__init__(self)
                self.called_old_publish = False
                self.called_new_publish = False

            def DoPublish(self, context, events):
                self.called_new_publish = True

            def _DoPublish(self, context, events):
                self.called_old_publish = True

        system = AccidentallyBothSystem()
        context = system.CreateDefaultContext()
        # This will trigger no deprecations, as the newer publish is called.
        system.Publish(context)
        self.assertTrue(system.called_new_publish)
        self.assertFalse(system.called_old_publish)

    def _DoPublish(self, context, event):
        # TODO(russt): Change this to declare a periodic event with a
        # callback instead of overriding _DoPublish, pending #9992.
        LeafSystem._DoPublish(self, context, event)

        pose_bundle = self.EvalAbstractInput(context, 0).get_value()

        for frame_i in range(pose_bundle.get_num_poses()):
            # SceneGraph currently sets the name in PoseBundle as
            #    "get_source_name::frame_name".
            [source_name, frame_name] = pose_bundle.get_name(frame_i)\
                .split("::")
            model_id = pose_bundle.get_model_instance_id(frame_i)
            # The MBP parsers only register the plant as a nameless source.
            # TODO(russt): Use a more textual naming convention here?
            self.vis[self.prefix][source_name][str(model_id)][frame_name]\
                .set_transform(pose_bundle.get_pose(frame_i).matrix())

    def __init__(self, window=None, open_position=0.107,
                 closed_position=0.002, force_limit=40,
                 update_period_sec=0.05):
        """"
        Args:
            window:          Optionally pass in a tkinter.Tk() object to add
                             these widgets to.  Default behavior is to create
                             a new window.
            update_period_sec: Specifies how often the window update() method
                             gets called.
            open_position:   Target position for the finger when open.
            closed_position: Target position for the gripper when closed.
            force_limit:     Force limit to send to Schunk WSG controller.
        """
        LeafSystem.__init__(self)
        self._DeclareVectorOutputPort("position", BasicVector(1),
                                      self.CalcPositionOutput)
        self._DeclareVectorOutputPort("force_limit", BasicVector(1),
                                      self.CalcForceLimitOutput)

        if window is None:
            self.window = tk.Tk()
            self.window.title(title)
        else:
            self.window = window

        # Schedule window updates in either case (new or existing window):
        self._DeclarePeriodicPublish(update_period_sec, 0.0)

        self._open_button = tk.Button(self.window, text="Open Gripper",
                                      state=tk.DISABLED,
                                      command=self.open)
        self._open_button.pack()
        self._close_button = tk.Button(self.window, text="Close Gripper",
                                       command=self.close)
        self._close_button.pack()

        self._open_state = True

        self._open_position = open_position
        self._closed_position = closed_position
        self._force_limit = force_limit

        self.window.bind("<space>", self._space_callback)

    def __init__(self, grab_focus=True):
        LeafSystem.__init__(self)
        self._DeclareVectorOutputPort("rpy_xyz", BasicVector(6),
                                      self._DoCalcOutput)
        self._DeclareVectorOutputPort("position", BasicVector(1),
                                      self.CalcPositionOutput)
        self._DeclareVectorOutputPort("force_limit", BasicVector(1),
                                      self.CalcForceLimitOutput)

        # Note: This timing affects the keyboard teleop performance. A larger
        #       time step causes more lag in the response.
        self._DeclarePeriodicPublish(0.01, 0.0)

        self.teleop_manager = TeleopMouseKeyboardManager(grab_focus=grab_focus)
        self.roll = self.pitch = self.yaw = 0
        self.x = self.y = self.z = 0
        self.gripper_max = 0.107
        self.gripper_min = 0.01
        self.gripper_goal = self.gripper_max

    def __init__(self,
                 meshcat_viz,
                 force_threshold=1e-2,
                 contact_force_scale=10,
                 plant=None):
        """
        Args:
            meshcat_viz: a MeshcatVisualizer object.
            force_threshold: contact forces whose norms are smaller than
                force_threshold are not displayed.
            contact_force_scale: a contact force with norm F (in Newtons) is
                displayed as a cylinder with length F/contact_force_scale
                (in meters).
            plant: the MultibodyPlant associated with meshcat_viz.scene_graph.
        """
        LeafSystem.__init__(self)
        assert plant is not None
        self._meshcat_viz = meshcat_viz
        self._force_threshold = force_threshold
        self._contact_force_scale = contact_force_scale
        self._plant = plant

        self.set_name('meshcat_contact_visualizer')
        self._DeclarePeriodicPublish(self._meshcat_viz.draw_period, 0.0)
        # Pose bundle (from SceneGraph) input port.
        self._DeclareAbstractInputPort("pose_bundle",
                                       AbstractValue.Make(PoseBundle(0)))
        # Contact results input port from MultibodyPlant
        self._DeclareAbstractInputPort(
            "contact_results", AbstractValue.Make(ContactResults()))

        # Make force cylinders smaller at initialization.
        self._force_cylinder_radial_scale = 1.
        self._force_cylinder_longitudinal_scale = 100.

        # This system has undeclared states, see #4330.
        # - All contacts (previous and current), of type `_ContactState`.
        self._contacts = []
        # - Unique key for contacts in meshcat.
        self._contact_key_counter = 0
        # - Previous time at which contact was published.
        self._t_previous = 0.

 def _DoHasDirectFeedthrough(self, input_port, output_port):
     # Test inputs.
     test.assertEqual(input_port, 0)
     test.assertEqual(output_port, 0)
     # Call base method to ensure we do not get recursion.
     base_return = LeafSystem._DoHasDirectFeedthrough(
         self, input_port, output_port)
     test.assertTrue(base_return is None)
     # Return custom methods.
     self.called_feedthrough = True
     return False

    def _DoPublish(self, context, event):
        LeafSystem._DoPublish(self, context, event)
        point_cloud_P = self.EvalAbstractInput(context, 0).get_value()

        # `Q` is a point in the point cloud.
        p_PQs = point_cloud_P.xyzs()
        # Use only valid points.
        valid = np.logical_not(np.isnan(p_PQs))
        valid = np.all(valid, axis=0)  # Reduce along XYZ axis.
        p_PQs = p_PQs[:, valid]
        if point_cloud_P.has_rgbs():
            rgbs = point_cloud_P.rgbs()[:, valid]
        else:
            # Need manual broadcasting.
            count = p_PQs.shape[1]
            rgbs = np.tile(np.array([self._default_rgb]).T, (1, count))
        # pydrake `PointCloud.rgbs()` are on [0..255], while meshcat
        # `PointCloud` colors are on [0..1].
        rgbs = rgbs / 255.  # Do not use in-place so we can promote types.
        # Send to meshcat.
        self._meshcat_viz[self._name].set_object(g.PointCloud(p_PQs, rgbs))
        self._meshcat_viz[self._name].set_transform(self._X_WP.matrix())

 def __init__(self):
     LeafSystem.__init__(self)
     self.called_publish = False
     self.called_feedthrough = False
     self.called_continuous = False
     self.called_discrete = False
     self.called_initialize = False
     self.called_per_step = False
     self.called_periodic = False
     # Ensure we have desired overloads.
     self._DeclarePeriodicPublish(1.0)
     self._DeclarePeriodicPublish(1.0, 0)
     self._DeclarePeriodicPublish(period_sec=1.0, offset_sec=0.)
     self._DeclarePeriodicDiscreteUpdate(
         period_sec=1.0, offset_sec=0.)
     self._DeclareInitializationEvent(
         event=PublishEvent(
             trigger_type=TriggerType.kInitialization,
             callback=self._on_initialize))
     self._DeclarePerStepEvent(
         event=PublishEvent(
             trigger_type=TriggerType.kPerStep,
             callback=self._on_per_step))
     self._DeclarePeriodicEvent(
         period_sec=1.0,
         offset_sec=0.0,
         event=PublishEvent(
             trigger_type=TriggerType.kPeriodic,
             callback=self._on_periodic))
     self._DeclareContinuousState(2)
     self._DeclareDiscreteState(1)
     # Ensure that we have inputs / outputs to call direct
     # feedthrough.
     self._DeclareInputPort(PortDataType.kVectorValued, 1)
     self._DeclareVectorInputPort(
         name="test_input", model_vector=BasicVector(1),
         random_type=None)
     self._DeclareVectorOutputPort(BasicVector(1), noop)

    def __init__(self,
                 scene_graph,
                 draw_period=0.033333,
                 prefix="drake",
                 zmq_url="default",
                 open_browser=None):
        """
        Args:
            scene_graph: A SceneGraph object.
            draw_period: The rate at which this class publishes to the
                visualizer.
            prefix: Appears as the root of the tree structure in the meshcat
                data structure
            zmq_url: Optionally set a url to connect to the visualizer.
                Use zmp_url="default" to the value obtained by running a
                single `meshcat-server` in another terminal.
                Use zmp_url=None or zmq_url="new" to start a new server (as a
                child of this process); a new web browser will be opened (the
                url will also be printed to the console).
                Use e.g. zmq_url="tcp://127.0.0.1:6000" to specify a
                specific address.
            open_browser: Set to True to open the meshcat browser url in your
                default web browser.  The default value of None will open the
                browser iff a new meshcat server is created as a subprocess.
                Set to False to disable this.

        Note: This call will not return until it connects to the
              meshcat-server.
        """
        LeafSystem.__init__(self)

        self.set_name('meshcat_visualizer')
        self._DeclarePeriodicPublish(draw_period, 0.0)

        # Pose bundle (from SceneGraph) input port.
        self._DeclareAbstractInputPort("lcm_visualization",
                                       AbstractValue.Make(PoseBundle(0)))

        if zmq_url == "default":
            zmq_url = "tcp://127.0.0.1:6000"
        elif zmq_url == "new":
            zmq_url = None

        if zmq_url is None and open_browser is None:
            open_browser = True

        # Set up meshcat.
        self.prefix = prefix
        if zmq_url is not None:
            print("Connecting to meshcat-server at zmq_url=" + zmq_url + "...")
        self.vis = meshcat.Visualizer(zmq_url=zmq_url)
        print("Connected to meshcat-server.")
        self._scene_graph = scene_graph

        if open_browser:
            webbrowser.open(self.vis.url())

        def on_initialize(context, event):
            self.load()

        self._DeclareInitializationEvent(
            event=PublishEvent(
                trigger_type=TriggerType.kInitialization,
                callback=on_initialize))