def __init__(self, world):
GLWidgetPlugin.__init__(self)
self.world = world
self.poseWidget = PointPoser()
self.robotWidget = RobotPoser(world.robot(0))
self.addWidget(self.poseWidget)
self.addWidget(self.robotWidget)
def keyboardfunc(self, c, x, y):
#Put your keyboard handler here
#the current example prints the config when [space] is pressed
if c == ' ':
config = self.robotWidget.get()
subconfig = self.subrobots[0].fromfull(config)
print("Config:", subconfig)
self.subrobots[0].setConfig(subconfig)
elif c == 'r':
self.subrobots[0].randomizeConfig()
self.robotWidget.set(self.robot.getConfig())
elif c == 'p':
config = self.robotWidget.get()
subconfig = self.subrobots[0].fromfull(config)
self.subrobots[0].setConfig(self.subrobots[0].fromfull(
self.startConfig))
settings = {
'type': "sbl",
'perturbationRadius': 0.5,
'bidirectional': 1,
'shortcut': 1,
'restart': 1,
'restartTermCond': "{foundSolution:1,maxIters:1000}"
}
plan = robotplanning.planToConfig(self.world,
self.subrobots[0],
subconfig,
movingSubset='all',
**settings)
plan.planMore(1000)
print(plan.getPath())
else:
GLWidgetPlugin.keyboardfunc(self, c, x, y)
def initialize(self):
GLWidgetPlugin.initialize(self)
self.images = {}
self.images['left'] = GLTexture("UI/Resources/left-arm.png")
self.images['right'] = GLTexture("UI/Resources/right-arm.png")
self.images['both'] = GLTexture("UI/Resources/both-arm.png")
return True
def multiwindow_template(world):
"""Tests multiple windows and views."""
vis.add("world",world)
vp = vis.getViewport()
vp.w,vp.h = 800,800
vis.setViewport(vp)
vis.setWindowTitle("vis.spin test: will close in 5 seconds...")
vis.spin(5.0)
#Now testing ability to re-launch windows
vis.setWindowTitle("Shown again. Close me to proceed.")
vis.spin(float('inf'))
vis.setWindowTitle("Dialog test. Close me to proceed.")
vp = vis.getViewport()
vp.w,vp.h = 400,600
vis.setViewport(vp)
vis.dialog()
vp.w,vp.h = 640,480
vis.setViewport(vp)
for i in range(3):
widgets = GLWidgetPlugin()
widgets.addWidget(RobotPoser(world.robot(0)))
vis.addPlugin(widgets)
vis.setWindowTitle("Split screen test")
vis.spin(float('inf'))
vis.setPlugin(None)
vis.setWindowTitle("Back to normal. Close me to quit.")
vis.dialog()
vis.kill()
def __init__(self, world, robot):
GLWidgetPlugin.__init__(self)
self.world = world
self.robot = robot
assert robot.index == 0, "Can only work with the 0'th robot in the world"
self.resolution = RedundancyResolutionGraph(world)
self.mode = 'interpolate'
self.configs = None
self.folder = None
self.settings = None
self.drawWorkspaceRoadmap = False
self.solveConstraint = True
self.clippingplanes = (0.1, 50)
self.rotationAsDepth = False
self.pointWidget = PointPoser()
self.xformWidget = TransformPoser()
self.roadmapDisplayList = CachedGLObject()
self.disconnectionDisplayList = CachedGLObject()
self.movie_frame = None
self.movie_rotate = False
self.walk_path = None
self.walk_workspace_path = None
self.walk_progress = None
self.temp_config = None
self.useboundary = None
def __init__(self,world):
GLWidgetPlugin.__init__(self)
self.world = world
self.poseWidget = PointPoser()
self.robotWidget = RobotPoser(world.robot(0))
self.addWidget(self.poseWidget)
self.addWidget(self.robotWidget)
self.add_action(self.print_config,'Print config',' ')
self.add_action(self.save_world,'Save world','s')
def __init__(self,taskGen):
GLWidgetPlugin.__init__(self)
self.world = taskGen.world
self.sendMilestone = False
self.initialPose = False
self.tuckArmPose = False
self.selectedLimb = "both"
self.gripperControl = False
self.gripperState = "open"
self.trackPosition = False
self.trackPosition_lastState = False
def multiwindow_template(world):
"""Tests multiple windows and views."""
vis.add("world", world)
vp = vis.getViewport()
vp.w, vp.h = 400, 600
vis.setViewport(vp)
vis.addText("label1", "This is Window 1", (20, 20))
vis.setWindowTitle("Window 1")
vis.show()
id1 = vis.getWindow()
print("First window ID:", id1)
id2 = vis.createWindow("Window 2")
vis.add("Lone point", [0, 0, 0])
vis.setViewport(vp)
vis.addText("label1", "This is Window 2", (20, 20))
print("Second window ID:", vis.getWindow())
vis.setWindow(id2)
vis.spin(float('inf'))
#restore back to 1 window, clear the text
vis.setWindow(id1)
vis.clearText()
vp = vis.getViewport()
vp.w, vp.h = 800, 800
vis.setViewport(vp)
vis.setWindowTitle("vis.spin test: will close in 5 seconds...")
vis.spin(5.0)
#Now testing ability to re-launch windows
vis.setWindowTitle("Shown again. Close me to proceed.")
vis.spin(float('inf'))
vis.setWindowTitle("Dialog test. Close me to proceed.")
vp = vis.getViewport()
vp.w, vp.h = 400, 600
vis.setViewport(vp)
vis.dialog()
vp.w, vp.h = 640, 480
vis.setViewport(vp)
for i in range(3):
widgets = GLWidgetPlugin()
widgets.addWidget(RobotPoser(world.robot(0)))
vis.addPlugin(widgets)
vis.setWindowTitle("Split screen test")
vis.spin(float('inf'))
vis.setPlugin(None)
vis.setWindowTitle("Back to normal. Close me to quit.")
vis.dialog()
vis.kill()
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 keyboardfunc(self, c, x, y):
#Put your keyboard handler here
#the current example prints the config when [space] is pressed
if c == 'h' or c == '?':
print("Controls:")
print("- [space]: print the widget's sub-robot configuration")
print("- r: randomize the sub-robot configuration")
print("- p: plan to the widget's sub-robot configuration")
print("- i: test the IK functions")
elif c == ' ':
config = self.robotWidget.get()
subconfig = self.subrobots[0].fromfull(config)
print("Config:", subconfig)
self.subrobots[0].setConfig(subconfig)
elif c == 'r':
self.subrobots[0].randomizeConfig()
self.robotWidget.set(self.robot.getConfig())
elif c == 'p':
config = self.robotWidget.get()
subconfig = self.subrobots[0].fromfull(config)
self.subrobots[0].setConfig(self.subrobots[0].fromfull(
self.startConfig))
settings = {
'type': "sbl",
'perturbationRadius': 0.5,
'bidirectional': 1,
'shortcut': 1,
'restart': 1,
'restartTermCond': "{foundSolution:1,maxIters:1000}"
}
plan = robotplanning.planToConfig(self.world,
self.subrobots[0],
subconfig,
movingSubset='all',
**settings)
plan.planMore(1000)
print(plan.getPath())
elif c == 'i':
link = self.subrobots[0].link(self.subrobots[0].numLinks() - 1)
print("IK solve for ee to go 10cm upward...")
obj = ik.objective(link,
local=[0, 0, 0],
world=vectorops.add(
link.getWorldPosition([0, 0, 0]),
[0, 0, 0.1]))
solver = ik.solver(obj)
res = solver.solve()
print(" result", res)
print(" residual", solver.getResidual())
print(self.robotWidget.set(self.robot.getConfig()))
else:
GLWidgetPlugin.keyboardfunc(self, c, x, y)
def keyboardfunc(self, c, x, y):
#Put your keyboard handler here
#the current example prints the config when [space] is pressed
if c == ' ':
config = self.robotWidget.get()
print "Config:", config
self.world.robot(0).setConfig(config)
elif c == 's':
fn = "widgets_test_world.xml"
print "Saving file to", fn
self.world.saveFile(fn)
else:
GLWidgetPlugin.keyboardfunc(self, c, x, y)
def initialize(self):
GLWidgetPlugin.initialize(self)
self.window.program.clearColor = [1, 1, 1, 1]
assert self.resolution.domain != None
print "Domain", self.resolution.domain
assert len(
self.resolution.ikTaskSpace) == 1, "Can only do one task, for now"
task = self.resolution.ikTaskSpace[0]
if task.orientation == 'variable':
#if a 2D problem and want to show depth, turn this to true
active = [
i for i, (a, b) in enumerate(zip(*self.resolution.domain)[:3])
if b != a
]
if len(active) <= 2:
self.rotationAsDepth = True
else:
self.rotationAsDepth = False
link = self.resolution.ikTemplate.objectives[0].link()
self.xformWidget.enableTranslation(True)
self.xformWidget.enableRotation(True)
print "Initial transform", self.robot.link(link).getTransform()
#self.xformWidget.set(*self.robot.link(link).getTransform())
self.addWidget(self.xformWidget)
elif task.orientation == 'axis':
self.rotationAsDepth = False
link = self.resolution.ikTemplate.objectives[0].link()
self.xformWidget.enableTranslation(True)
self.xformWidget.enableRotation(True)
print "Initial transform", self.robot.link(link).getTransform()
#self.xformWidget.set(*self.robot.link(link).getTransform())
self.addWidget(self.xformWidget)
else:
self.rotationAsDepth = False
link = self.resolution.ikTemplate.objectives[0].link()
local, world = self.resolution.ikTemplate.objectives[
0].getPosition()
print "Initial position", self.resolution.robot.link(
link).getWorldPosition(local)
self.pointWidget.set(
self.resolution.robot.link(link).getWorldPosition(local))
self.addWidget(self.pointWidget)
return True
def coordinates_template(world):
"""Tests integration with the coordinates module."""
#add the world to the visualizer
vis.add("world", world)
coordinates.setWorldModel(world)
#add the coordinate Manager to the visualizer
vis.add("coordinates", coordinates.manager())
vis.setWindowTitle("Coordinates visualiation test")
if MANUAL_EDITING:
#manually adds a poser, and adds a callback whenever the widget changes
widgets = GLWidgetPlugin()
widgets.addWidget(RobotPoser(world.robot(0)))
#update the coordinates every time the widget changes
widgets.widgetchangefunc = (lambda self: coordinates.updateFromWorld())
vis.pushPlugin(widgets)
if not MULTITHREADED:
vis.loop(callback=None, setup=vis.show)
else:
vis.show()
while vis.shown():
time.sleep(0.01)
else:
vis.edit(("world", world.robot(0).getName()))
if not MULTITHREADED:
def callback(coordinates=coordinates):
coordinates.updateFromWorld()
vis.loop(callback=callback, setup=vis.show)
else:
vis.show()
while vis.shown():
vis.lock()
#reads the coordinates from the world
coordinates.updateFromWorld()
vis.unlock()
time.sleep(0.01)
#quit the visualization thread nicely
vis.kill()
def __init__(self, world):
GLWidgetPlugin.__init__(self)
self.world = world
self.robot = world.robot(0)
self.subrobots = []
for i in range(6):
self.subrobots.append(
SubRobotModel(self.robot, list(range(6 + i * 6, 12 + i * 6))))
(res, val) = editors.run(
editors.SelectionEditor("subrobot",
self.subrobots[0]._links,
description="sub robot links",
world=world,
robot=self.robot))
if res:
print("Return value", val)
self.subrobots[0]._links = val
self.startConfig = self.robot.getConfig()
self.robotWidget = RobotPoser(world.robot(0))
self.robotWidget.setActiveDofs(self.subrobots[0]._links)
self.addWidget(self.robotWidget)
def __init__(self, world, object):
GLWidgetPlugin.__init__(self)
from klampt.model.collide import WorldCollider
from klampt.plan.rigidobjectcspace import RigidObjectCSpace
from klampt import ObjectPoser
self.world = world
self.object = object
self.space = RigidObjectCSpace(object, collider=WorldCollider(world))
self.costFunction = None
self.start = None
self.goal = None
self.startWidget = ObjectPoser(self.object)
self.goalWidget = ObjectPoser(self.object)
self.addWidget(self.startWidget)
self.addWidget(self.goalWidget)
self.optimizingPlanner = True
self.path = []
self.G = None
print "Showing again..."
vis.show()
while vis.shown():
time.sleep(0.01)
"""
print "Doing a dialog..."
vis.setWindowTitle("Dialog test")
print "calling dialog()"
vis.dialog()
print "Doing a split screen program..."
vp.w, vp.h = 640, 480
vis.setViewport(vp)
for i in range(3):
widgets = GLWidgetPlugin()
widgets.addWidget(RobotPoser(world.robot(0)))
#update the coordinates every time the widget changes
widgets.widgetchangefunc = (lambda self: coordinates.updateFromWorld())
vis.addPlugin(widgets)
vis.setWindowTitle("Split screen test")
vis.show()
while vis.shown():
time.sleep(0.1)
print "Showing a dialog, back to normal..."
vis.setPlugin(None)
vis.dialog()
print "Showing again, back to normal..."
vis.setWindowTitle("Basic visualization test")
vis.show()
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 motionfunc(self, x, y, dx, dy):
res = GLWidgetPlugin.motionfunc(self, x, y, dx, dy)
self.do_click(x, y)
return res
def initialize(self):
GLWidgetPlugin.initialize(self)
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)
def mousefunc(self, button, state, x, y):
GLWidgetPlugin.mousefunc(self, button, state, x, y)
self.do_click(x, y)
def __init__(self, taskGen):
GLWidgetPlugin.__init__(self)
self.world = taskGen.world
self.sendMilestone = False
self.initialPose = False
