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])
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)
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.
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]))
