class GLRedundancyProgram(GLBaseClass): def __init__(self, world, robot): if KLAMPT_VERSION >= 0.7: GLBaseClass.__init__(self) else: GLBaseClass.__init__(self, "Redundancy resolution test") 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.drawJointLimitColors = True self.jointLimitColoringRatio = 0.1 self.jointLimitColor = (1, 0.5, 0, 1) self.jointLimits = robot.getJointLimits() 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 initialize(self): GLBaseClass.initialize(self) if KLAMPT_VERSION >= 0.7: self.window.program.clearColor = [1, 1, 1, 1] else: self.clearColor = [1, 1, 1, 1] assert self.resolution.domain != None print "Domain", self.resolution.domain if len(self.resolution.domain[0]) == 6: #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()) if KLAMPT_VERSION >= 0.7: self.addWidget(self.xformWidget) else: self.widgetMaster.add(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)) if KLAMPT_VERSION >= 0.7: self.addWidget(self.pointWidget) else: self.widgetMaster.add(self.pointWidget) return True def run(self): """Standardized interface for running program""" if KLAMPT_VERSION >= 0.7: vp = vis.getViewport() #Square screen #vp.w,vp.h = 800,800 #For saving HD quality movies vp.w, vp.h = 1024, 768 vp.clippingplanes = self.clippingplanes vis.setViewport(vp) #vis.run(program) vis.setPlugin(self) vis.show() while vis.shown(): time.sleep(0.1) vis.setPlugin(None) vis.kill() else: #Square screen #self.width,self.height = 800,800 #For saving HD quality movies self.width, self.height = 1024, 768 GLBaseClass.run(self) def workspaceToPoint(self, x): if self.rotationAsDepth: return (x[0], x[4], x[2]) else: return x[:3] def drawConfig(self, q=None): if self.drawJointLimitColors: testq = q if q is None: testq = self.robot.getConfig() oldColors = [] for i in xrange(self.robot.numLinks()): oldColors.append(self.robot.link(i).appearance().getColor()) if self.robot.getJointType(i) == 'normal': a, b = self.jointLimits[0][i], self.jointLimits[1][i] u = 1.0 u = min(1, (testq[i] - a) / (self.jointLimitColoringRatio * (b - a))) u = min(1, (b - testq[i]) / (self.jointLimitColoringRatio * (b - a))) if u != 1: u = max(u, 0.0) c = vectorops.add( vectorops.mul(oldColors[i], u), vectorops.mul(self.jointLimitColor, 1 - u)) self.robot.link(i).appearance().setColor(*c) if q is not None: self.robot.setConfig(q) self.robot.drawGL() if self.drawJointLimitColors: for i in xrange(self.robot.numLinks()): self.robot.link(i).appearance().setColor(*oldColors[i]) def display(self): if self.configs == None: self.drawConfig(None) else: for q in self.configs: self.drawConfig(q) if self.walk_workspace_path != None: if 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.drawConfig(self.temp_config) 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) #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 = workspace_distance(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 len(self.resolution.domain[0]) == 6: 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() orientation = so3.from_moment(mbest) 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 len(self.resolution.domain[0]) == 6: 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: print "Drawing new display list for moment", mbest self.disconnectionDisplayLists_by_orientation[ mbest] = CachedGLObject() orientation = so3.from_moment(mbest) self.disconnectionDisplayLists_by_orientation[mbest].draw( drawDisconnections, args=(orientation, )) 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) GLBaseClass.display(self) def mousefunc(self, button, state, x, y): GLBaseClass.mousefunc(self, button, state, x, y) self.do_click(x, y) def do_click(self, x, y): if self.pointWidget.hasFocus(): p = self.pointWidget.get() if len(self.resolution.domain[0]) == 6: x = p + [0, 0, 0] else: x = p if self.mode == 'interpolate': if self.solveConstraint: qOrig = self.configs[ 0] if self.configs is not None else self.robot.getConfig( ) qx = self.resolution.eval(x, qOrig) self.configs = [qx if qx is not None else qOrig] else: self.configs = self.resolution.interpolate(x, multi=True) else: self.resolution.interpolate(x, multi=True) self.configs = self.resolution.lastqs self.refresh() if self.xformWidget.hasFocus(): R, t = self.xformWidget.get() m = so3.moment(R) x = t + m if self.mode == 'interpolate': if self.solveConstraint: qOrig = self.configs[ 0] if self.configs is not None else self.robot.getConfig( ) qx = self.resolution.eval(x, qOrig) self.configs = [qx if qx is not None else qOrig] else: self.configs = self.resolution.interpolate(x, multi=True) else: self.resolution.interpolate(x, multi=True) self.configs = self.resolution.lastqs self.refresh() def motionfunc(self, x, y, dx=None, dy=None): if KLAMPT_VERSION >= 0.7: GLBaseClass.motionfunc(self, x, y, dx, dy) self.do_click(x, y) else: GLBaseClass.motionfunc(self, dx, dy) self.do_click(self.lastx, self.lasty) def keyboardfunc(self, c, x, y): if c == 'h': print "Keyboard help:" print "- x: verifies edge checks for existing resolution" print "- i: toggles between interpolation mode and graph inspection mode" print "- g: toggles drawing the workspace graph" print "- w: performs a walk to a random workspace node" print "- m: saves a real-time movie" print "- M: saves a 360 spin movie" elif c == 'x': self.resolution.verify() self.disconnectionDisplayList.markChanged() elif c == 'i': if self.mode == 'interpolate': self.mode = 'inspect' else: self.mode = 'interpolate' print "Toggled visualization mode to", self.mode elif c == 'g': self.drawWorkspaceRoadmap = not self.drawWorkspaceRoadmap elif c == 'b': if self.useboundary: self.useboundary = False else: self.useboundary = True self.disconnectionDisplayList.markChanged() elif c == 'm': if self.movie_frame is None: self.movie_frame = 0 self.movie_rotate = False else: self.movie_frame = None elif c == 'M': if self.movie_frame is None: self.movie_frame = 0 self.movie_rotate = True else: self.movie_frame = None elif c == 'w': import random resolved = [] for iw, d in self.resolution.Gw.nodes(data=True): if d.get('config', None) is not None: resolved.append(iw) if self.walk_workspace_path == None: #draw boundaries transparent now self.disconnectionDisplayList.markChanged() for iters in range(10): wtgt = random.choice(resolved) #TEMP: place widgets far away for capturing video far = [20] * 3 self.pointWidget.set(far) R, t = self.xformWidget.get() self.xformWidget.set(R, far[:3]) #get current config if self.configs != None: self.robot.setConfig(self.configs[0]) try: x, p = self.resolution.walkPath(wtgt) except Exception as e: import traceback print "Exception", e traceback.print_exc() print "WalkPath failed, trying with a new random point" continue self.walk_workspace_path = None if p != None: t = 0 times = [] for i, q in enumerate(p): times.append(t) if i + 1 < len(p): t += linf_distance(q, p[i + 1], self.resolution.spinJoints) #t += self.robot.distance(q,p[i+1]) #t += 0.1 self.walk_workspace_path = trajectory.Trajectory(times, x) self.walk_path = trajectory.RobotTrajectory( self.robot, times, p) self.walk_progress = 0 if self.temp_config == None: self.temp_config = p[0] break self.refresh() def idle(self): t0 = time.time() if self.movie_frame is not None: numframes = 180 if self.movie_rotate: self.window.program.view.camera.rot[ 2] += math.pi * 2 / numframes self.window.program.save_screen("frame%03d.ppm" % (self.movie_frame)) self.movie_frame += 1 if self.movie_rotate and self.movie_frame >= numframes: self.movie_frame = None if self.walk_path != None: self.walk_progress += 0.02 if self.walk_progress >= self.walk_path.times[-1]: self.configs = [self.walk_path.milestones[-1]] self.walk_path = None else: #self.configs = [self.walk_path.eval(self.walk_progress)] self.configs = [ self.resolution.eval( self.walk_workspace_path.eval(self.walk_progress)) ] if self.configs[0] == None: self.configs = [] u = self.walk_progress / self.walk_path.times[-1] qstraight = self.resolution.solve( self.temp_config, vectorops.interpolate( self.walk_workspace_path.milestones[0], self.walk_workspace_path.milestones[-1], u), testfeasibility=False) if qstraight and ( self.resolution.ikTemplate.feasibilityTest == None or self.resolution.ikTemplate.feasibilityTest(qstraight)): self.temp_config = qstraight self.refresh() t1 = time.time() if t1 - t0 < 0.02: time.sleep(0.02 - (t1 - t0))
class GLStabilityPlugin(GLWidgetPlugin): def __init__(self,folder="FrictionCones/StanfordMicroSpineUnit"): GLWidgetPlugin.__init__(self) self.factory = GeneralizedFrictionConeFactory() self.factory.load(folder) self.contacts = [] self.widgets = self.klamptwidgetmaster self.cmWidget = PointPoser() self.cmWidget.set((0,0,0.5)) self.contactWidgets = [TransformPoser()] self.widgets.add(self.cmWidget) self.widgets.add(self.contactWidgets[0]) self.wrenches = None self.contactSize = 0.02 self.feasible = None def display(self): GLWidgetPlugin.display(self) glEnable(GL_LIGHTING) if self.feasible == True: gldraw.setcolor(0,1,0,lighting=True) else: gldraw.setcolor(1,0,0,lighting=True) glPushMatrix() glTranslatef(*self.cmWidget.get()) radius = 0.05 gldraw.box([-radius]*3,[radius]*3) glPopMatrix() for w in self.contactWidgets: T = w.get() glPushMatrix() try: mat = zip(*se3.homogeneous(T)) mat = sum([list(coli) for coli in mat],[]) glMultMatrixf(mat) self.factory.forceTemplate.drawGL(self.contactSize) except: import traceback traceback.print_exc() exit(-1) glPopMatrix() if self.wrenches != None and len(self.wrenches) == len(self.contactWidgets): glDisable(GL_LIGHTING) glDisable(GL_DEPTH_TEST) glColor3f(1,0.5,0) glBegin(GL_LINES) for w,widget in zip(self.wrenches,self.contactWidgets): p = widget.get()[1] glVertex3fv(p) glVertex3fv(vectorops.madd(p,(w[0],w[1],w[2]),1.0/9.8)) glEnd() glColor3f(0,0.5,1) glBegin(GL_LINES) for w,widget in zip(self.wrenches,self.contactWidgets): p = widget.get()[1] glVertex3fv(p) glVertex3fv(vectorops.madd(p,(w[3],w[4],w[5]),1.0/9.8)) glEnd() glEnable(GL_DEPTH_TEST) def solve(self): solver = GeneralizedStabilitySolver(self.factory) for w in self.contactWidgets: print "Transform",w.get() solver.addContactPatch(w.get(),self.contactSize) t0 = time.time() print "Mass 4, CM",self.cmWidget.get() solver.setGravityWrench(4,self.cmWidget.get()) self.wrenches = solver.solve() self.feasible = (self.wrenches != None) print "Feasible:",self.feasible if self.feasible: print self.wrenches assert len(self.wrenches) == len(self.contactWidgets) def transform(self,T): self.cmWidget.set(se3.apply(T,self.cmWidget.get())) for w in self.contactWidgets: w.set(*se3.mul(T,w.get())) def keyboardfunc(self,c,x,y): if c == '?': print "Help:" print "-?: display this message" print "-s: solve for stability" print "-a: add a new contact" print "-r: remove the currrently hovered contact" print "-<: rotate the entire setup ccw about the y axis" print "->: rotate the entire setup cw about the y axis" elif c == 's': self.solve() elif c == 'a': self.contactWidgets.append(TransformPoser()) self.widgets.add(self.contactWidgets[-1]) self.solve() elif c == 'r': removed = False for i,w in enumerate(self.contactWidgets): if w.hasHighlight(): self.widgets.remove(w) self.contactWidgets.pop(i) removed = True break if not removed: print "Warning, no currently hovered widget" else: self.solve() elif c == '<': T = (so3.rotation([0,1,0],math.radians(-5)),[0]*3) self.transform(T) self.solve() elif c == '>': T = (so3.rotation([0,1,0],math.radians(5)),[0]*3) self.transform(T) self.solve() self.refresh()