def display(self):
robot = self.world.robot(0)
oldColors = []
for i in range(robot.numLinks()):
#c = robot.link(i).appearance().getColor()
c = [0.5, 0.5, 0.5, 1.0]
oldColors.append(c)
robot.link(i).appearance().setColor(c[0], c[1], c[2], 0.5)
GLWidgetPlugin.display(self)
for i in range(robot.numLinks()):
c = oldColors[i]
robot.link(i).appearance().setColor(c[0], c[1], c[2], c[3])
def display(self):
GLWidgetPlugin.display(self)
glEnable(GL_LIGHTING)
for i in range(self.world.numRobots()):
self.world.robot(i).drawGL()
for i in range(self.world.numRigidObjects()):
if i != self.object.index:
self.world.rigidObject(i).drawGL()
for i in range(self.world.numTerrains()):
self.world.terrain(i).drawGL()
self.object.setTransform(*self.startWidget.get())
self.object.drawGL()
self.object.setTransform(*self.goalWidget.get())
self.object.drawGL()
if self.path:
#draw path
glDisable(GL_LIGHTING)
glColor3f(0, 1, 0)
glBegin(GL_LINE_STRIP)
for q in self.path:
glVertex3f(q[0], q[1], q[2])
glEnd()
glEnable(GL_LIGHTING)
for q in self.path:
self.object.setTransform(*self.space.configToTransform(q))
self.object.drawGL()
else:
if self.start:
self.object.setTransform(
*self.space.configToTransform(self.start))
self.object.drawGL()
if self.goal:
self.object.setTransform(
*self.space.configToTransform(self.goal))
self.object.drawGL()
if self.G:
#draw graph
V, E = self.G
glDisable(GL_LIGHTING)
glEnable(GL_BLEND)
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
glColor4f(0, 0, 0, 0.5)
glPointSize(3.0)
glBegin(GL_POINTS)
for v in V:
glVertex3f(v[0], v[1], v[2])
glEnd()
glColor4f(0.5, 0.5, 0.5, 0.5)
glBegin(GL_LINES)
for (i, j) in E:
glVertex3f(V[i][0], V[i][1], V[i][2])
glVertex3f(V[j][0], V[j][1], V[j][2])
glEnd()
glDisable(GL_BLEND)
glEnable(GL_LIGHTING)
return True
def display(self):
if self.configs == None:
self.robot.drawGL()
else:
for q in self.configs:
self.robot.setConfig(q)
self.robot.drawGL()
if self.walk_workspace_path != None:
if self.temp_config:
self.robot.setConfig(self.temp_config)
glEnable(GL_BLEND)
#glDisable(GL_DEPTH_TEST)
for i in xrange(self.robot.numLinks()):
self.robot.link(i).appearance().setColor(1, 0, 0, 0.5)
self.robot.drawGL()
for i in xrange(self.robot.numLinks()):
self.robot.link(i).appearance().setColor(0.5, 0.5, 0.5, 1)
#glEnable(GL_DEPTH_TEST)
glDisable(GL_BLEND)
glColor3f(1, 1, 0)
glLineWidth(5.0)
glBegin(GL_LINE_STRIP)
for w in self.walk_workspace_path.milestones:
if len(w) == 2:
glVertex2f(w[0], w[1])
else:
glVertex3f(w[0], w[1], w[2])
glEnd()
glLineWidth(1.0)
assert len(
self.resolution.ikTaskSpace) == 1, "Can only do one task for now"
task = self.resolution.ikTaskSpace[0]
#DEBUG
if hasattr(self.resolution, 'delaunay'):
delaunay = self.resolution.delaunay
delaunay_pts = self.resolution.delaunay_pts
delaunay_epts = self.resolution.delaunay_epts
delaunay_others = self.resolution.delaunay_others
ptset = set(tuple(v) for v in delaunay_pts)
assert len(ptset) == len(
delaunay_pts), "Duplicate points! %d unique / %d" % (
len(ptset), len(delaunay_pts))
glDisable(GL_LIGHTING)
glDisable(GL_DEPTH_TEST)
glColor3f(0, 0, 0)
glLineWidth(1.0)
import random
for s in delaunay.simplices:
"""
glBegin(GL_LINE_LOOP)
for v in s:
glVertex3fv(delaunay_pts[v])
glEnd()
"""
centroid = vectorops.div(
vectorops.add(*[delaunay_pts[v] for v in s]), len(s))
assert len(s) == 3
glBegin(GL_LINES)
for i, v in enumerate(s):
neighbors = [w for j, w in enumerate(s) if i < j]
for w in neighbors:
if (v in delaunay_epts and w in delaunay_others) or (
w in delaunay_epts and v in delaunay_others):
glColor3f(1, 1, 1)
else:
glColor3f(0, 0, 0)
glVertex3fv(
vectorops.interpolate(delaunay_pts[v], centroid,
0.1))
glVertex3fv(
vectorops.interpolate(delaunay_pts[w], centroid,
0.1))
glEnd()
glColor3f(0, 1, 0)
glPointSize(5.0)
glBegin(GL_POINTS)
for v in delaunay_epts:
glVertex3fv(delaunay_pts[v])
glEnd()
glColor3f(0, 0, 1)
glPointSize(5.0)
glBegin(GL_POINTS)
for v in delaunay_others:
glVertex3fv(delaunay_pts[v])
glEnd()
glEnable(GL_DEPTH_TEST)
#draw workspace graph
if self.drawWorkspaceRoadmap:
def drawWorkspaceRoadmap(orientation=None):
G = self.resolution.Gw
if orientation is not None:
Rclosest, G = self.resolution.extractOrientedSubgraph(
orientation)
print G.number_of_nodes()
if not self.resolution.hasResolution():
glDisable(GL_LIGHTING)
glPointSize(5.0)
glColor3f(1, 0, 0)
glBegin(GL_POINTS)
for n, d in G.nodes_iter(data=True):
glVertex3fv(self.workspaceToPoint(d['params']))
glEnd()
glColor3f(1, 0.5, 0)
glBegin(GL_LINES)
for (i, j) in G.edges_iter():
glVertex3fv(self.workspaceToPoint(G.node[i]['params']))
glVertex3fv(self.workspaceToPoint(G.node[j]['params']))
glEnd()
else:
#maxn = max(len(d['qlist']) for n,d in G.nodes_iter(data=True))
#maxe = max(len(d['qlist']) for i,j,d in G.edges_iter(data=True))
#maxn = max(maxn,1)
#maxe = max(maxe,1)
glDisable(GL_LIGHTING)
glPointSize(5.0)
glBegin(GL_POINTS)
for n, d in G.nodes_iter(data=True):
if not self.resolution.isResolvedNode(n):
continue
#u = float(len(d['qlist']))/float(maxn)
#nsubset = sum(1 for iq in d['qlist'] if iq in self.rr.Qsubgraph)
#if nsubset > 1:
# glColor3f(1,0,1)
#else:
# glColor3f(u,nsubset*0.5,0)
glColor3f(1, 0.5, 0)
glVertex3fv(self.workspaceToPoint(d['params']))
glEnd()
glBegin(GL_LINES)
for (i, j, d) in G.edges_iter(data=True):
if not self.resolution.isResolvedNode(
i) or not self.resolution.isResolvedNode(j):
continue
#nsubset = sum(1 for (ia,ib) in d['qlist'] if (ia in self.Qsubgraph and ib in self.Qsubgraph))
#u = float(len(d['qlist']))/float(maxe)
r, g, b = 1, 1, 0
if not self.resolution.isResolvedEdge(i, j):
r, g, b = 1, 0, 1
else:
qd = self.robot.distance(G.node[i]['config'],
G.node[j]['config'])
wd = self.resolution.workspaceDistance(
G.node[i]['params'], G.node[j]['params'])
u = 1.0 - math.exp(-max(0.0, 2.0 * qd / wd - 1.0))
if u > 0.8:
r, g, b = 1, 1.0 - u * 5.0, 0.0
else:
r, g, b = u / 0.8, 1, 0.0
#assert (nsubset >=1) == self.resolution.isResolvedEdge(i,j),"Mismatch between Qsubgraph and resolution?"
glColor3f(r, g, b)
glVertex3fv(self.workspaceToPoint(G.node[i]['params']))
glVertex3fv(self.workspaceToPoint(G.node[j]['params']))
glEnd()
"""
glEnable(GL_LIGHTING)
q0 = self.robot.getConfig()
for iw,d in self.rr.Gw.nodes_iter(data=True):
qs = [iq for iq in d['qlist'] if iq in self.rr.Qsubgraph]
if len(qs) > 1:
for iq in qs:
self.robot.setConfig(self.rr.Gq.node[iq]['config'])
self.robot.drawGL()
self.robot.setConfig(q0)
glDisable(GL_LIGHTING)
"""
if task.numConstraints > 3:
if not hasattr(self, 'roadmapDisplayLists_by_orientation'):
self.roadmapDisplayLists_by_orientation = dict()
self.lastROrientation = None
self.lastRMbest = None
orientation = self.xformWidget.get()[0]
if orientation != self.lastROrientation:
mbest = self.resolution.closestOrientation(orientation)
self.lastRMbest = mbest
else:
mbest = self.lastRMbest
if mbest not in self.roadmapDisplayLists_by_orientation:
print "Drawing new display list for moment", mbest
self.roadmapDisplayLists_by_orientation[
mbest] = CachedGLObject()
self.roadmapDisplayLists_by_orientation[mbest].draw(
drawWorkspaceRoadmap, args=(orientation, ))
else:
#there looks to be a bug in GL display lists for drawing lines...
#self.roadmapDisplayList.draw(drawWorkspaceRoadmap)
drawWorkspaceRoadmap()
else:
#render boundaries only
def drawDisconnections(orientation=None):
bmin, bmax = self.resolution.domain
active = [
i for i, (a, b) in enumerate(zip(bmin, bmax)[:3]) if b != a
]
if self.useboundary == None:
self.useboundary = (len(active) == 2)
verts, faces = self.resolution.computeDiscontinuities(
self.useboundary, orientation)
if len(active) == 3:
glEnable(GL_LIGHTING)
glEnable(GL_BLEND)
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
glDisable(GL_CULL_FACE)
#if self.walk_workspace_path != None:
# gldraw.setcolor(1,0,1,0.25)
#else:
# gldraw.setcolor(1,0,1,0.5)
for tri in faces:
tverts = [verts[v] for v in tri]
centroid = vectorops.div(vectorops.add(*tverts),
len(tri))
params = [
(x - a) / (b - a)
for (x, a,
b) in zip(centroid, self.resolution.domain[0],
self.resolution.domain[1])
]
if self.walk_workspace_path != None:
gldraw.setcolor(params[0], params[1], params[2],
0.25)
else:
gldraw.setcolor(params[0], params[1], params[2],
1.0)
gldraw.triangle(*tverts)
glEnable(GL_CULL_FACE)
else:
glDisable(GL_LIGHTING)
glEnable(GL_BLEND)
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
glColor4f(1, 0, 1, 0.5)
glLineWidth(4.0)
glBegin(GL_LINES)
for s in faces:
spts = verts[s[0]], verts[s[1]]
glVertex3f(spts[0][0], 0, spts[0][1])
glVertex3f(spts[1][0], 0, spts[1][1])
glEnd()
glLineWidth(1.0)
#draw stabbing lines
"""
glDisable(GL_LIGHTING)
glColor3f(1,0,0)
glBegin(GL_LINES)
for (i,j,d) in self.resolution.Gw.edges_iter(data=True):
if self.resolution.isResolvedEdge(i,j):
continue
if not self.resolution.isResolvedNode(i) or not self.resolution.isResolvedNode(j):
continue
glVertex3fv(self.workspaceToPoint(self.resolution.Gw.node[i]['params']))
glVertex3fv(self.workspaceToPoint(self.resolution.Gw.node[j]['params']))
glEnd()
"""
if task.numConstraints > 3:
if not hasattr(self,
'disconnectionDisplayLists_by_orientation'):
self.disconnectionDisplayLists_by_orientation = dict()
self.lastOrientation = None
self.lastMbest = None
orientation = self.xformWidget.get()[0]
if orientation != self.lastOrientation:
mbest = self.resolution.closestOrientation(orientation)
self.lastMbest = mbest
else:
mbest = self.lastMbest
if mbest not in self.disconnectionDisplayLists_by_orientation:
self.disconnectionDisplayLists_by_orientation[
mbest] = CachedGLObject()
self.disconnectionDisplayLists_by_orientation[mbest].draw(
drawDisconnections, args=(mbest, ))
else:
self.disconnectionDisplayList.draw(drawDisconnections)
bmin, bmax = self.resolution.domain
#draw workspace bound
glDisable(GL_LIGHTING)
glColor3f(1, 0.5, 0)
gldraw.box(bmin[:3], bmax[:3], lighting=False, filled=False)
GLWidgetPlugin.display(self)