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