def __init__(self, ax=None, show=None):
        PyPlotVisualizer.__init__(self, ax=ax, show=show)
        self.DeclareInputPort(PortDataType.kVectorValued, 6)
        self.ax.set_aspect("equal")
        self.ax.set_xlim(-2, 2)
        self.ax.set_ylim(-1, 1)

        self.length = .25  # moment arm (meters)

        self.base = np.vstack((1.2 * self.length * np.array([1, -1, -1, 1, 1]),
                               0.025 * np.array([1, 1, -1, -1, 1])))
        self.pin = np.vstack((0.005 * np.array([1, 1, -1, -1, 1]),
                              .1 * np.array([1, 0, 0, 1, 1])))
        a = np.linspace(0, 2 * np.pi, 50)
        self.prop = np.vstack(
            (self.length / 1.5 * np.cos(a), .1 + .02 * np.sin(2 * a)))

        # yapf: disable
        self.base_fill = self.ax.fill(
            self.base[0, :], self.base[1, :], zorder=1, edgecolor="k",
            facecolor=[.6, .6, .6])
        self.left_pin_fill = self.ax.fill(
            self.pin[0, :], self.pin[1, :], zorder=0, edgecolor="k",
            facecolor=[0, 0, 0])
        self.right_pin_fill = self.ax.fill(
            self.pin[0, :], self.pin[1, :], zorder=0, edgecolor="k",
            facecolor=[0, 0, 0])
        self.left_prop_fill = self.ax.fill(
            self.prop[0, :], self.prop[0, :], zorder=0, edgecolor="k",
            facecolor=[0, 0, 1])
        self.right_prop_fill = self.ax.fill(
            self.prop[0, :], self.prop[0, :], zorder=0, edgecolor="k",
            facecolor=[0, 0, 1])
Exemple #2
0
    def __init__(self, ax=None):
        PyPlotVisualizer.__init__(self, ax=ax)
        self.DeclareInputPort(PortDataType.kVectorValued, 8)
        self.ax.set_aspect("equal")
        self.ax.set_xlim(0, 2)
        self.ax.set_ylim(-2, 2)

        # Draw the ground.
        self.ax.plot([-50, 50], [0, 0], "k")

        a = np.linspace(0, 2 * np.pi, 50)
        radius = 0.1
        self.hip_fill = self.ax.fill(radius * np.sin(a),
                                     radius * np.cos(a),
                                     zorder=1,
                                     edgecolor="k",
                                     facecolor=[.6, .6, .6])
        self.hip = copy.copy(self.hip_fill[0].get_path().vertices)

        self.leg_line = [self.ax.plot([0, 0], [0, -1], "k")[0]]
        self.leg_data = [self.leg_line[0].get_xydata().T]
        for i in range(1, 13):
            self.leg_line.append(
                self.ax.plot(
                    0.1 * np.array(
                        [np.sin((i - 1) * np.pi / 2.),
                         np.sin(i * np.pi / 2.)]),
                    -.2 - .7 / 13 * np.array([i - 1, i]), "k")[0])
            self.leg_data.append(self.leg_line[i].get_xydata().T)
Exemple #3
0
    def __init__(self):
        PyPlotVisualizer.__init__(self)
        self.DeclareInputPort(PortDataType.kVectorValued, 2)

        self.ax.set_xlim(*self.XLIM)
        self.ax.set_ylim(*self.YLIM)
        self.ax.set_aspect("auto")

        self._make_background()
        self.brick = Brick.add_to_axes(self.ax)
    def __init__(self, size):
        PyPlotVisualizer.__init__(self)
        self.DeclareInputPort(PortDataType.kVectorValued, size)

        self.ax.set_xlim(*self.XLIM)
        self.ax.set_ylim(*self.YLIM)
        self.ax.set_aspect('auto')

        self._make_background()

        self.patch = plt.Rectangle((0.0, 0.0),
                                   self.PATCH_WIDTH, self.PATCH_HEIGHT,
                                   fc='#A31F34', ec='k')
        self.patch.set_x(-self.PATCH_WIDTH / 2)  # Center at x.
Exemple #5
0
    def __init__(self, ax=None, show=None):
        PyPlotVisualizer.__init__(self, ax=ax, show=show)
        self.set_name("pendulum_visualizer")
        self.DeclareInputPort("state", PortDataType.kVectorValued, 2)

        self.ax.set_xlim([-1.2, 1.2])
        self.ax.set_ylim([-1.2, 1.2])
        # yapf: disable
        self.base = self.ax.fill(
            self.base_x, self.base_y, zorder=1, facecolor=(.3, .6, .4),
            edgecolor="k")

        # arm_x and arm_y are closed (last element == first element), but don't
        # pass the last element to the fill command, because it gets closed
        # anyhow (and we want the sizes to match for the update).
        self.arm = self.ax.fill(
            self.arm_x[0:-1], self.arm_y[0:-1], zorder=0, facecolor=(.9, .1, 0),
            edgecolor="k")
        self.center_of_mass = self.ax.plot(
            0, -self.ac1, zorder=1, color="b", marker="o", markersize=14)
    def __init__(self,
                 scene_graph,
                 draw_period=1. / 30,
                 T_VW=np.array([[1., 0., 0., 0.], [0., 0., 1., 0.],
                                [0., 0., 0., 1.]]),
                 xlim=[-1., 1],
                 ylim=[-1, 1],
                 facecolor=[1, 1, 1],
                 use_random_colors=False,
                 substitute_collocated_mesh_files=True,
                 ax=None,
                 show=None):
        """
        Args:
            scene_graph: A SceneGraph object.
            draw_period: The rate at which this class publishes to the
                visualizer.
            T_VW: The view projection matrix from world to view coordinates.
            xlim: View limit into the scene.
            ylim: View limit into the scene.
            facecolor: Passed through to figure() and sets background color.
                Both color name strings and RGB triplets are allowed. Defaults
                to white.
            use_random_colors: If set to True, will render each body with a
                different color. (Multiple visual elements on the same body
                will be the same color.)
            substitute_collocated_mesh_files: If True, then a mesh file
                specified with an unsupported filename extension may be
                replaced by a file of the same base name in the same directory,
                but with a supported filename extension.  Currently only .obj
                files are supported.
            ax: If supplied, the visualizer will draw onto those axes instead
                of creating a new set of axes. The visualizer will still change
                the view range and figure size of those axes.
            show: Opens a window during initialization / publish iff True.
                Default is None, which implies show=True unless
                matplotlib.get_backend() is 'template'.
        """
        default_size = matplotlib.rcParams['figure.figsize']
        scalefactor = (ylim[1] - ylim[0]) / (xlim[1] - xlim[0])
        figsize = (default_size[0], default_size[0] * scalefactor)

        PyPlotVisualizer.__init__(self,
                                  facecolor=facecolor,
                                  figsize=figsize,
                                  ax=ax,
                                  draw_period=draw_period,
                                  show=show)
        self.set_name('planar_multibody_visualizer')

        self._scene_graph = scene_graph
        self._T_VW = T_VW

        # Pose bundle (from SceneGraph) input port.
        # TODO(tehbelinda): Rename the `lcm_visualization` port to match
        # SceneGraph once its output port has been updated. See #12214.
        self._pose_bundle_port = self.DeclareAbstractInputPort(
            "lcm_visualization", AbstractValue.Make(PoseBundle(0)))

        self.ax.axis('equal')
        self.ax.axis('off')

        # Achieve the desired view limits.
        self.ax.set_xlim(xlim)
        self.ax.set_ylim(ylim)
        default_size = self.fig.get_size_inches()
        self.fig.set_size_inches(figsize[0], figsize[1])

        # Populate body patches.
        self._build_body_patches(use_random_colors,
                                 substitute_collocated_mesh_files)

        # Populate the body fill list -- which requires doing most of a draw
        # pass, but with an ax.fill() command to initialize the draw patches.
        # After initialization, we can then use in-place replacement of vertex
        # positions. The body fill list stores the ax patch objects in the
        # order they were spawned (i.e. by body, and then by order of view_
        # patches). Drawing the tree should update them by iterating over
        # bodies and patches in the same order.
        self._body_fill_dict = {}
        X_WB_initial = RigidTransform.Identity()
        for full_name in self._patch_Blist.keys():
            patch_Wlist, view_colors = self._get_view_patches(
                full_name, X_WB_initial)
            self._body_fill_dict[full_name] = []
            for patch_W, color in zip(patch_Wlist, view_colors):
                # Project the full patch the first time, to initialize a vertex
                # list with enough space for any possible convex hull of this
                # vertex set.
                patch_V = self._project_patch(patch_W)
                body_fill = self.ax.fill(patch_V[0, :],
                                         patch_V[1, :],
                                         zorder=0,
                                         edgecolor='k',
                                         facecolor=color,
                                         closed=True)[0]
                self._body_fill_dict[full_name].append(body_fill)
                # Then update the vertices for a more accurate initial draw.
                self._update_body_fill_verts(body_fill, patch_V)
    def test_recording(self):
        visualizer = PyPlotVisualizer()

        # Assert that we start with no recordings. This uses private API for
        # testing _recorded_contexts and should not be used publicly.
        self.assertEqual(len(visualizer._recorded_contexts), 0)
        visualizer.start_recording()

        # Artificially produce some specific contexts.
        times = [0.003, 0.2, 1.1, 1.12]
        context = visualizer.AllocateContext()
        for time in times:
            context.SetTime(time)
            visualizer.Publish(context)

        # Check that there are now recorded contexts with matching times.
        visualizer.stop_recording()
        self.assertEqual(len(visualizer._recorded_contexts), len(times))
        for i, time in enumerate(times):
            self.assertEqual(time, visualizer._recorded_contexts[i].get_time())

        ani = visualizer.get_recording_as_animation()
        self.assertIsInstance(ani, animation.FuncAnimation)

        visualizer.reset_recording()
        self.assertEqual(len(visualizer._recorded_contexts), 0)
    def __init__(self, n_quadrotors=0, n_balls=0, ax=None):
        PyPlotVisualizer.__init__(self, ax=ax)
        self.n_quadrotors = n_quadrotors
        for _ in range(self.n_quadrotors):
            self.DeclareInputPort(PortDataType.kVectorValued, 6)
        self.n_balls = n_balls
        for _ in range(self.n_balls):
            self.DeclareInputPort(PortDataType.kVectorValued, 4)
        self.ax.set_aspect("equal")
        self.ax.set_xlim(-8, 8)
        self.ax.set_ylim(-4, 4)

        # Initialize quadrotor visualization parameters
        self.length = .25  # moment arm (meters)

        self.base = np.vstack((1.2 * self.length * np.array([1, -1, -1, 1, 1]),
                               0.025 * np.array([1, 1, -1, -1, 1])))
        self.pin = np.vstack((0.005 * np.array([1, 1, -1, -1, 1]),
                              .1 * np.array([1, 0, 0, 1, 1])))
        a = np.linspace(0, 2 * np.pi, 50)
        self.prop = np.vstack(
            (self.length / 1.5 * np.cos(a), .1 + .02 * np.sin(2 * a)))

        # yapf: disable
        self.base_fill = []
        self.left_pin_fill = []
        self.right_pin_fill = []
        self.left_prop_fill = []
        self.right_prop_fill = []

        for _ in range(self.n_quadrotors):
            self.base_fill.append(self.ax.fill(
                self.base[0, :], self.base[1, :], zorder=1, edgecolor="k",
                facecolor=[.6, .6, .6]))
            self.left_pin_fill.append(self.ax.fill(
                self.pin[0, :], self.pin[1, :], zorder=0, edgecolor="k",
                facecolor=[0, 0, 0]))
            self.right_pin_fill.append(self.ax.fill(
                self.pin[0, :], self.pin[1, :], zorder=0, edgecolor="k",
                facecolor=[0, 0, 0]))
            self.left_prop_fill.append(self.ax.fill(
                self.prop[0, :], self.prop[0, :], zorder=0, edgecolor="k",
                facecolor=[0, 0, 1]))
            self.right_prop_fill.append(self.ax.fill(
                self.prop[0, :], self.prop[0, :], zorder=0, edgecolor="k",
                facecolor=[0, 0, 1]))
        # yapf: enable

        # Initialize ball visualization parameters
        self.radius = 0.1

        a = np.linspace(0, 2 * np.pi, 50)
        self.ball = np.vstack(
            (self.radius * np.cos(a), self.radius * np.sin(a)))

        self.ball_fill = []
        for _ in range(self.n_balls):
            self.ball_fill.append(
                self.ax.fill(self.ball[0, :],
                             self.ball[0, :],
                             zorder=0,
                             edgecolor="k",
                             facecolor=[1, 0, 0]))