def __init__(self):
PyPlotVisualizer.__init__(self)
self.set_name('pendulum_visualizer')
self._DeclareInputPort(PortDataType.kVectorValued, 2)
self.ax.set_xlim([-1.2, 1.2])
self.ax.set_ylim([-1.2, 1.2])
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, num_particles, xlim, ylim, draw_timestep):
PyPlotVisualizer.__init__(self, draw_timestep)
self._DeclareInputPort(PortDataType.kVectorValued, 2 * num_particles)
self.num_particles = num_particles
self.ax.set_xlim(xlim)
self.ax.set_ylim(ylim)
zero = array.array('d', (0, ) * num_particles)
self.lines, = self.ax.plot(zero, zero, 'b.')
def __init__(self, num_particles, xlim, ylim, draw_timestep):
PyPlotVisualizer.__init__(self, draw_timestep)
self.DeclareInputPort(PortDataType.kVectorValued, 2*num_particles)
self.num_particles = num_particles
self.ax.set_xlim(xlim)
self.ax.set_ylim(ylim)
zero = array.array('d', (0,)*num_particles)
self.lines, = self.ax.plot(zero, zero, 'b.')
def __init__(self, num_samples, bins, xlim, ylim, draw_timestep):
PyPlotVisualizer.__init__(self, draw_timestep)
for i in range(0, num_samples):
self._DeclareInputPort(PortDataType.kVectorValued, 1)
self.num_samples = num_samples
self.bins = bins
self.data = array.array('d', (0, ) * num_samples)
self.limits = xlim
self.ax.set_xlim(xlim)
self.ax.set_ylim(ylim)
self.patches = None
def __init__(self, ax=None):
PyPlotVisualizer.__init__(self, ax=ax)
self.set_name('pendulum_visualizer')
self.DeclareInputPort(PortDataType.kVectorValued, 2)
self.ax.set_xlim([-1.2, 1.2])
self.ax.set_ylim([-1.2, 1.2])
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,
rbtree,
Tview=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,
figsize_multiplier=1.,
ax=None):
default_size = matplotlib.rcParams['figure.figsize']
scalefactor = (ylim[1] - ylim[0]) / (xlim[1] - xlim[0])
figsize = (figsize_multiplier * default_size[0],
default_size[0] * scalefactor * figsize_multiplier)
PyPlotVisualizer.__init__(self,
facecolor=facecolor,
figsize=figsize,
ax=ax)
self.set_name('planar_rigid_body_visualizer')
self.rbtree = rbtree
self.Tview = Tview
self.Tview_pinv = np.linalg.pinv(self.Tview)
self.DeclareInputPort(
PortDataType.kVectorValued,
self.rbtree.get_num_positions() + self.rbtree.get_num_velocities())
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()
scalefactor = (ylim[1] - ylim[0]) / (xlim[1] - xlim[0])
self.fig.set_size_inches(default_size[0],
default_size[0] * scalefactor)
# Populate body patches
self.buildViewPatches(use_random_colors)
# Populate the body fill list -- which requires doing most of
# a draw pass, but with an ax.fill() command rather than
# an in-place replacement of vertex positions to initialize
# the draw patches.
# The body fill list stores the ax patch objects in the
# order they were spawned (i.e. by body, and then by
# order of viewPatches). Drawing the tree should update them
# by iterating over bodies and patches in the same order.
self.body_fill_list = []
q0 = np.zeros((self.rbtree.get_num_positions(), ))
kinsol = self.rbtree.doKinematics(q0)
n_bodies = self.rbtree.get_num_bodies()
for body_i in range(n_bodies):
tf = self.rbtree.relativeTransform(kinsol, 0, body_i)
viewPatches, viewColors = self.getViewPatches(body_i, tf)
for patch, color in zip(viewPatches, viewColors):
self.body_fill_list += self.ax.fill(patch[0, :],
patch[1, :],
zorder=0,
edgecolor='k',
facecolor=color,
closed=True)
def __init__(self,
scene_graph,
draw_period=0.033333,
Tview=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,
ax=None):
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_timestep=draw_period)
self.set_name('planar_multibody_visualizer')
self._scene_graph = scene_graph
self.Tview = Tview
self.Tview_pinv = np.linalg.pinv(self.Tview)
# Pose bundle (from SceneGraph) input 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()
scalefactor = (ylim[1] - ylim[0]) / (xlim[1] - xlim[0])
self.fig.set_size_inches(default_size[0],
default_size[0] * scalefactor)
# Populate body patches
self.buildViewPatches(use_random_colors)
# Populate the body fill list -- which requires doing most of
# a draw pass, but with an ax.fill() command rather than
# an in-place replacement of vertex positions to initialize
# the draw patches.
# The body fill list stores the ax patch objects in the
# order they were spawned (i.e. by body, and then by
# order of viewPatches). Drawing the tree should update them
# by iterating over bodies and patches in the same order.
self.body_fill_dict = {}
n_bodies = len(self.viewPatches.keys())
tf = np.eye(4)
for full_name in self.viewPatches.keys():
viewPatches, viewColors = self.getViewPatches(full_name, tf)
self.body_fill_dict[full_name] = []
for patch, color in zip(viewPatches, viewColors):
# 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_proj = np.dot(self.Tview, patch)
self.body_fill_dict[full_name] += self.ax.fill(
patch_proj[0, :],
patch_proj[1, :],
zorder=0,
edgecolor='k',
facecolor=color,
closed=True)