def plot_polygon(id, pattern):
"""Plot a polygon with a given database id into a file with the given pattern."""
m_polygon = model.Polygon.get(model.Polygon.id == id)
pyplot.figure(figsize=(20, 20))
pyplot.gcf().clear()
draw_polygon(pyplot.gca(), m_polygon.as_geometry())
pyplot.title('polygon {polygon_id}'.format(polygon_id=id))
pyplot.draw()
pyplot.savefig(pattern.format(id=id),
dpi='figure',
bbox_inches='tight',
pad_inches=0.1)
def save_polygon(basename: str, polygon: Polygon, trapezoid_path: List[Point] = None,
lee_preparata_path: List[Point] = None, s: int = None, t: int = None):
"""Plot a polygon (with paths) and save it to a file."""
if trapezoid_path is None and lee_preparata_path is None and polygon in save_polygon.polygons:
return
if trapezoid_path is None and lee_preparata_path is None:
with open(basename + '.polygon', 'w') as f:
for p in polygon.points:
print('{0:f} {1:f}'.format(p.x, p.y), file=f)
if polygon not in save_polygon.polygons and not (trapezoid_path is None and lee_preparata_path is None):
save_polygon(basename, polygon)
if polygon not in save_polygon.polygons:
save_polygon.polygons.append(polygon)
pyplot.ioff()
fig, ax = pyplot.subplots()
draw_polygon(ax, polygon)
pyplot.draw()
if trapezoid_path is not None:
ax.plot(list([p.x for p in trapezoid_path]), list([p.y for p in trapezoid_path]),
color='blue', linestyle='--', marker='*', markersize=2, zorder=99, linewidth=.75, alpha=.5)
ax.annotate('trapezoid', xy=trapezoid_path[0].tuple(),
xytext=(
trapezoid_path[0].x + size / ticks / 20,
trapezoid_path[0].y + size / ticks / 20),
fontsize=font_size, color='blue', alpha=.8)
if lee_preparata_path is not None:
ax.plot(list([p.x for p in lee_preparata_path]), list([p.y for p in lee_preparata_path]),
color='orange', linestyle=':', marker='s', markersize=2, zorder=99, linewidth=.75, alpha=.5)
ax.annotate('lee_preparata', xy=lee_preparata_path[0].tuple(),
xytext=(
lee_preparata_path[0].x + size / ticks / 20,
lee_preparata_path[0].y - size / ticks / 20),
fontsize=font_size, color='orange', alpha=.8)
image_pattern = '{basename}.png'
image_path_pattern = '{basename}:{s:03d}->{t:03d}.png'
if lee_preparata_path is None and trapezoid_path is None:
fig.savefig(image_pattern.format(basename=basename),
dpi=dpi, bbox_inches='tight', pad_inches=.01)
else:
fig.savefig(image_path_pattern.format(basename=basename, s=s, t=t),
dpi=dpi, bbox_inches='tight', pad_inches=.01)
pyplot.close()
def plot_run(id, pattern):
"""Plot a run with a given database id into a file with the given pattern."""
m_run = model.Run.get(model.Run.id == id)
pyplot.figure(figsize=(20, 20))
pyplot.gcf().clear()
draw_polygon(pyplot.gca(), m_run.polygon.as_geometry())
m_points = (m_run.run_path_points.select(
model.RunPathPoint, model.Algorithm,
model.Point).join(model.PolygonPoint).join(model.Point).switch(
model.RunPathPoint).join(model.Algorithm))
points = []
algorithm = None
def plot(pts):
pyplot.plot([p[0] for p in pts], [p[1] for p in pts],
color=plot_colors[algorithm],
marker='x',
markersize=3,
label=algorithm)
for m_point in m_points:
if algorithm is None or algorithm != m_point.algorithm.name:
if points:
plot(points)
points = []
algorithm = m_point.algorithm.name
points.append((float(m_point.polygon_point.point.x),
float(m_point.polygon_point.point.y)))
if points:
plot(points)
pyplot.legend(bbox_to_anchor=(0., 1.02, 1., .102),
loc=3,
ncol=4,
mode='expand',
borderaxespad=0.)
pyplot.title('run {run_id}; polygon {polygon_id}'.format(
run_id=id, polygon_id=m_run.polygon_id))
pyplot.draw()
pyplot.savefig(pattern.format(id=id),
dpi='figure',
bbox_inches='tight',
pad_inches=0.1)
def update_polygon(self):
self.handle = None
try:
vertices = self.contact_set.compute_static_equilibrium_polygon()
self.handle = draw_polygon(
[(x[0], x[1], self.z) for x in vertices],
normal=[0, 0, 1], color=self.color)
except Exception as e:
print "SEPDrawer:", e
def update_polygon(self):
self.handle = None
try:
vertices = self.contact_set.compute_static_equilibrium_polygon()
self.handle = draw_polygon(
[(x[0], x[1], self.z) for x in vertices],
normal=[0, 0, 1], color=(0.5, 0., 0.5, 0.5))
except Exception as e:
print "SEPDrawer:", e
def update_polygon(self):
self.handle = None
try:
vertices = self.contact_set.compute_zmp_support_area(
self.stance.com.p, [0, 0, self.z])
self.handle = draw_polygon(
[(x[0], x[1], self.z) for x in vertices],
normal=[0, 0, 1], color=(0.0, 0.0, 0.5, 0.5))
except Exception as e:
print "ZMPSupportAreaDrawer:", e
def update_polygon(self):
self.handle = None
try:
vertices = self.contact_set.compute_zmp_support_area(
self.stance.com.p, [0, 0, self.z], method=self.method)
self.handle = draw_polygon(
[(x[0], x[1], self.z) for x in vertices],
normal=[0, 0, 1], color=(0.0, 0.5, 0.5, 0.5))
except Exception as e:
print "ZMPSupportAreaDrawer:", e
def draw(self, apex, size=1., combined='g-#', color=None, linewidth=2):
"""
Draw cone with apex at a given world position.
Parameters
----------
apex : array
Position of the origin of the cone in world coordinates.
size : scalar, optional
Scale factor.
combined : string, optional
Drawing spec in matplotlib fashion. Default is 'g-#'.
color : char or triplet, optional
Color letter or RGB values, default is 'g' for green.
linewidth : scalar
Thickness of drawn line.
Returns
-------
handles : list of openravepy.GraphHandle
OpenRAVE graphical handles. Must be stored in some variable,
otherwise the drawn object will vanish instantly.
"""
assert len(apex) == 3, "apex is not a 3D point"
assert len(self.rays[0]) == 3, "only 3D cones can be drawn"
rays = self.rays
if color is None:
color = matplotlib_to_rgba(combined[0])
if type(color) is str:
color = matplotlib_to_rgba(color)
env = get_openrave_env()
normal = average(rays, axis=0)
normal /= norm(normal)
section = [apex + ray * size / dot(normal, ray) for ray in rays]
handles = draw_polygon(points=section,
normal=normal,
combined=combined,
color=color)
edges = vstack([[apex, vertex] for vertex in section])
edge_color = array(color) * 0.7
edge_color[3] = 1.
handles.append(
env.drawlinelist(edges, linewidth=linewidth, colors=edge_color))
return handles
def draw(self, apex, size=1., combined='g-#', color=None, linewidth=2):
"""
Draw cone with apex at a given world position.
INPUT:
- ``apex`` -- position of the apex of the cone in world coordinates
- ``size`` -- scale factor (default: 1.)
- ``combined`` -- drawing spec in matplotlib fashion (default: 'g-#')
- ``color`` -- color letter or RGBA tuple
- ``linewidth`` -- thickness of the edges of the cone
OUTPUT:
A list of OpenRAVE handles. Must be stored in some variable, otherwise
the drawn object will vanish instantly.
"""
assert len(apex) == 3, "apex is not a 3D point"
assert len(self.rays[0]) == 3, "only 3D cones can be drawn"
rays = self.rays
if color is None:
color = matplotlib_to_rgba(combined[0])
if type(color) is str:
color = matplotlib_to_rgba(color)
env = get_openrave_env()
normal = average(rays, axis=0)
normal /= norm(normal)
section = [apex + ray * size / dot(normal, ray) for ray in rays]
handles = draw_polygon(
points=section, normal=normal, combined=combined, color=color)
edges = vstack([[apex, vertex] for vertex in section])
edge_color = array(color) * 0.7
edge_color[3] = 1.
handles.append(env.drawlinelist(
edges, linewidth=linewidth, colors=edge_color))
return handles
on_key_press.file = os.path.splitext(sys.argv[1])[0] + '.pointtest'
if os.path.exists(on_key_press.file):
with open(on_key_press.file, 'r') as f:
points = True
for l in f:
if l.startswith('%'):
points = False
continue
if points:
continue
point, rest = l.strip().split('<')
edge, visible = rest.split(':')
point = int(point)
edge = int(edge)
visible = int(visible) == 1
if point not in on_key_press.data:
on_key_press.data[point] = dict()
on_key_press.data[point][edge] = visible
fig, ax = plt.subplots()
draw_polygon(ax, polygon)
fig.canvas.mpl_connect('button_release_event', on_mouse_press)
fig.canvas.mpl_connect('key_release_event', on_key_press)
plt.show()
def save_polygon(basename: str,
polygon: Polygon,
trapezoid_path: List[Point] = None,
delaunay_path: List[Point] = None,
makestep_path: List[Point] = None,
s: int = None,
t: int = None):
"""Plot a polygon (with paths) and save it to a file."""
if trapezoid_path is None and delaunay_path is None and polygon in save_polygon.polygons:
return
if polygon not in save_polygon.polygons and (trapezoid_path is not None
or delaunay_path is not None):
save_polygon(basename, polygon)
if polygon not in save_polygon.polygons:
save_polygon.polygons.append(polygon)
pyplot.ioff()
fig, ax = pyplot.subplots()
draw_polygon(ax, polygon)
pyplot.draw()
if trapezoid_path is not None:
ax.plot(list([p.x for p in trapezoid_path]),
list([p.y for p in trapezoid_path]),
color='magenta',
linestyle='--',
marker='*',
markersize=2,
zorder=99,
linewidth=.75,
alpha=.5)
ax.annotate('trapezoid',
xy=trapezoid_path[0].tuple(),
xytext=(trapezoid_path[0].x + size / ticks / 20,
trapezoid_path[0].y + size / ticks / 20),
fontsize=font_size,
color='magenta',
alpha=.8)
if delaunay_path is not None:
ax.plot(list([p.x for p in delaunay_path]),
list([p.y for p in delaunay_path]),
color='green',
linestyle=':',
marker='s',
markersize=2,
zorder=99,
linewidth=.75,
alpha=.5)
ax.annotate('delaunay',
xy=delaunay_path[0].tuple(),
xytext=(delaunay_path[0].x + size / ticks / 20,
delaunay_path[0].y - size / ticks / 20),
fontsize=font_size,
color='green',
alpha=.8)
if makestep_path is not None:
ax.plot(list([p.x for p in makestep_path]),
list([p.y for p in makestep_path]),
color='red',
linestyle='-.',
marker='^',
markersize=2,
zorder=99,
linewidth=.75,
alpha=.5)
ax.annotate('makestep',
xy=makestep_path[0].tuple(),
xytext=(makestep_path[0].x + size / ticks / 20,
makestep_path[0].y - 3 * size / ticks / 20),
fontsize=font_size,
color='red',
alpha=.8)
image_pattern = '{basename}.png'
image_path_pattern = '{basename}:{s:03d}->{t:03d}.png'
if delaunay_path is None and trapezoid_path is None and makestep_path is None:
fig.savefig(image_pattern.format(basename=basename),
dpi=dpi,
bbox_inches='tight',
pad_inches=.01)
else:
fig.savefig(image_path_pattern.format(basename=basename, s=s, t=t),
dpi=dpi,
bbox_inches='tight',
pad_inches=.01)
pyplot.close()
