def worldOffset(self, dir): """Offsets this direction by a vector in world coordinates""" if self._frame == None: self._localCoordinates = vectorops.add(self._localCoordinates, dir) else: self._localCoordinates = vectorops.add( so3.apply(so3.inv(self._frame.worldCoordinates()[0]), self._localCoordinates), dir)
def worldOffset(self, dir): """Offsets this direction by a vector in world coordinates""" if self._frame == None: self._localCoordinates = vectorops.add(self._localCoordinates, dir) else: self._localCoordinates = vectorops.add( so3.apply(so3.inv(self._frame.worldCoordinates()[0]), self._localCoordinates), dir )
def make_outline(self): left = [] right = [] for i in xrange(len(self.path)): s1l = None s1r = None s2l = None s2r = None if i > 0: d = vectorops.unit( vectorops.sub(self.path[i], self.path[i - 1])) ofs = (self.gutter * d[1], -self.gutter * d[0]) s1l = (d, ofs) s1r = (d, (-ofs[0], -ofs[1])) if i + 1 < len(self.path): d = vectorops.unit( vectorops.sub(self.path[i + 1], self.path[i])) ofs = (self.gutter * d[1], -self.gutter * d[0]) s2l = (d, ofs) s2r = (d, (-ofs[0], -ofs[1])) if i == 0: left.append(tuple(vectorops.add(self.path[i], s2l[1]))) right.append(tuple(vectorops.add(self.path[i], s2r[1]))) elif i + 1 == len(self.path): left.append(tuple(vectorops.add(self.path[i], s1l[1]))) right.append(tuple(vectorops.add(self.path[i], s1r[1]))) else: #figure out intersections if vectorops.dot(s1r[0], s2r[1]) < 0: #inner junction, find line intersection right.append( tuple( vectorops.add( self.path[i], ray_ray_intersection(s1r[1], s1r[0], s2r[1], s2r[0])))) else: #outer junction right.append(tuple(vectorops.add(self.path[i], s1r[1]))) right.append(tuple(vectorops.add(self.path[i], s2r[1]))) if vectorops.dot(s1l[0], s2l[1]) < 0: #inner junction, find line intersection left.append( tuple( vectorops.add( self.path[i], ray_ray_intersection(s1l[1], s1l[0], s2l[1], s2l[0])))) else: #outer junction left.append(tuple(vectorops.add(self.path[i], s1l[1]))) left.append(tuple(vectorops.add(self.path[i], s2l[1]))) self.outline = left + list(reversed(right))
def mul(T1,T2): """Composes two transformations.""" (R1,t1) = T1 (R2,t2) = T2 R = so3.mul(R1,R2) t = vectorops.add(so3.apply(R1,t2),t1) return (R,t)
def motionfunc(self,x,y,dx,dy): if self.dragging: if self.modifiers & GLUT_ACTIVE_CTRL: R,t = self.camera.matrix() delta = so3.apply(so3.inv(R),[float(dx)*self.camera.dist/self.width,-float(dy)*self.camera.dist/self.width,0]) self.camera.tgt = vectorops.add(self.camera.tgt,delta) elif self.modifiers & GLUT_ACTIVE_SHIFT: self.camera.dist *= math.exp(dy*0.01) else: self.camera.rot[2] += float(dx)*0.01 self.camera.rot[1] += float(dy)*0.01 self.refresh()
def segment_closest_point(s, x): """Given a segment s=(a,b) and a point x, returns a tuple containing the closest point parameter in [0,1] and the closest point on s""" dir = vectorops.sub(s[1], s[0]) d = vectorops.sub(x, s[0]) dp = vectorops.dot(d, dir) dnorm2 = vectorops.dot(dir, dir) if dp < 0: return (0.0, s[0]) if dp > dnorm2: return (1.0, s[1]) proj = vectorops.add(s[0], vectorops.mul(dir, dp / dnorm2)) return (dp / dnorm2, proj)
def motionfunc(self,x,y): dx = x - self.lastx dy = y - self.lasty if self.modifiers & GLUT_ACTIVE_CTRL: R,t = self.camera.matrix() delta = so3.apply(so3.inv(R),[float(dx)*self.camera.dist/self.width,-float(dy)*self.camera.dist/self.width,0]) self.camera.tgt = vectorops.add(self.camera.tgt,delta) elif self.modifiers & GLUT_ACTIVE_SHIFT: self.camera.dist *= math.exp(dy*0.01) else: self.camera.rot[2] += float(dx)*0.01 self.camera.rot[1] += float(dy)*0.01 self.lastx = x self.lasty = y glutPostRedisplay()
def drawLabelRaw(self,point,textList,color): #assert not self.makingDisplayList,"drawText must be called outside of display list" for i,text in enumerate(textList): if i+1 < len(textList): text = text+"," if glcommon._GLUTAvailable: glRasterPos3f(*point) glColor3f(0,0,0) glDisable(GL_LIGHTING) glDisable(GL_DEPTH_TEST) gldraw.glutBitmapString(GLUT_BITMAP_HELVETICA_10,text) glEnable(GL_DEPTH_TEST) elif glcommon._PyQtAvailable: glColor3f(0,0,0) glDisable(GL_DEPTH_TEST) self.window.renderText(point[0],point[1],point[2],text) glEnable(GL_DEPTH_TEST) point = vectorops.add(point,[0,0,-0.05])
def localOffset(self, dir): """Offsets this direction by a vector in local coordinates""" self._localCoordinates = vectorops.add(self._localCoordinates, dir)
def draw(self,world=None): """Draws the specified item in the specified world. If name is given and text_hidden != False, then the name of the item is shown.""" if self.hidden: return item = self.item name = self.name #set appearance if not self.useDefaultAppearance and hasattr(item,'appearance'): if not hasattr(self,'oldAppearance'): self.oldAppearance = item.appearance().clone() if self.customAppearance != None: print "Changing appearance of",name item.appearance().set(self.customAppearance) elif "color" in self.attributes: print "Changing color of",name item.appearance().setColor(*self.attributes["color"]) if hasattr(item,'drawGL'): item.drawGL() elif len(self.subAppearances)!=0: for n,app in self.subAppearances.iteritems(): app.widget = self.widget app.draw(world) elif isinstance(item,coordinates.Point): def drawRaw(): glDisable(GL_DEPTH_TEST) glDisable(GL_LIGHTING) glEnable(GL_POINT_SMOOTH) glPointSize(self.attributes.get("size",5.0)) glColor4f(*self.attributes.get("color",[0,0,0,1])) glBegin(GL_POINTS) glVertex3f(0,0,0) glEnd() glEnable(GL_DEPTH_TEST) #write name self.displayCache[0].draw(drawRaw,[so3.identity(),item.worldCoordinates()]) if name != None: self.drawText(name,vectorops.add(item.worldCoordinates(),[0,0,-0.05])) elif isinstance(item,coordinates.Direction): def drawRaw(): glDisable(GL_LIGHTING) glDisable(GL_DEPTH_TEST) L = self.attributes.get("length",0.15) source = [0,0,0] glColor4f(*self.attributes.get("color",[0,1,1,1])) glBegin(GL_LINES) glVertex3f(*source) glVertex3f(*vectorops.mul(item.localCoordinates(),L)) glEnd() glEnable(GL_DEPTH_TEST) #write name self.displayCache[0].draw(drawRaw,item.frame().worldCoordinates(),parameters = item.localCoordinates()) if name != None: self.drawText(name,vectorops.add(vectorops.add(item.frame().worldCoordinates()[1],item.worldCoordinates()),[0,0,-0.05])) elif isinstance(item,coordinates.Frame): t = item.worldCoordinates() if item.parent() != None: tp = item.parent().worldCoordinates() else: tp = se3.identity() tlocal = item.relativeCoordinates() def drawRaw(): glDisable(GL_DEPTH_TEST) glDisable(GL_LIGHTING) glLineWidth(2.0) gldraw.xform_widget(tlocal,self.attributes.get("length",0.1),self.attributes.get("width",0.01)) glLineWidth(1.0) #draw curve between frame and parent if item.parent() != None: d = vectorops.norm(tlocal[1]) vlen = d*0.5 v1 = so3.apply(tlocal[0],[-vlen]*3) v2 = [vlen]*3 #glEnable(GL_BLEND) #glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA) #glColor4f(1,1,0,0.5) glColor3f(1,1,0) gldraw.hermite_curve(tlocal[1],v1,[0,0,0],v2,0.03) #glDisable(GL_BLEND) glEnable(GL_DEPTH_TEST) #For some reason, cached drawing is causing OpenGL problems #when the frame is rapidly changing #self.displayCache[0].draw(drawRaw,transform=tp, parameters = tlocal) glPushMatrix() glMultMatrixf(sum(zip(*se3.homogeneous(tp)),())) drawRaw() glPopMatrix() #write name if name != None: self.drawText(name,se3.apply(t,[-0.05]*3)) elif isinstance(item,coordinates.Transform): #draw curve between frames t1 = item.source().worldCoordinates() if item.destination() != None: t2 = item.destination().worldCoordinates() else: t2 = se3.identity() d = vectorops.distance(t1[1],t2[1]) vlen = d*0.5 v1 = so3.apply(t1[0],[-vlen]*3) v2 = so3.apply(t2[0],[vlen]*3) def drawRaw(): glDisable(GL_DEPTH_TEST) glDisable(GL_LIGHTING) glColor3f(1,1,1) gldraw.hermite_curve(t1[1],v1,t2[1],v2,0.03) glEnable(GL_DEPTH_TEST) #write name at curve self.displayCache[0].draw(drawRaw,transform=None,parameters = (t1,t2)) if name != None: self.drawText(name,spline.hermite_eval(t1[1],v1,t2[1],v2,0.5)) else: types = resource.objectToTypes(item,world) if isinstance(types,(list,tuple)): #ambiguous, still need to figure out what to draw validtypes = [] for t in types: if t == 'Config': if world != None and len(t) == world.robot(0).numLinks(): validtypes.append(t) elif t=='Vector3': validtypes.append(t) elif t=='RigidTransform': validtypes.append(t) if len(validtypes) > 1: print "Unable to draw item of ambiguous types",validtypes return if len(validtypes) == 0: print "Unable to draw any of types",types return types = validtypes[0] if types == 'Config': if world: robot = world.robot(0) if not self.useDefaultAppearance: oldAppearance = [robot.link(i).appearance().clone() for i in xrange(robot.numLinks())] for i in xrange(robot.numLinks()): robot.link(i).appearance().set(self.customAppearance) oldconfig = robot.getConfig() robot.setConfig(item) robot.drawGL() robot.setConfig(oldconfig) if not self.useDefaultAppearance: for (i,app) in enumerate(oldAppearance): robot.link(i).appearance().set(app) else: print "Unable to draw Config's without a world" elif types == 'Vector3': def drawRaw(): glDisable(GL_LIGHTING) glEnable(GL_POINT_SMOOTH) glPointSize(self.attributes.get("size",5.0)) glColor4f(*self.attributes.get("color",[0,0,0,1])) glBegin(GL_POINTS) glVertex3f(0,0,0) glEnd() self.displayCache[0].draw(drawRaw,[so3.identity(),item]) if name != None: self.drawText(name,vectorops.add(item,[0,0,-0.05])) elif types == 'RigidTransform': def drawRaw(): gldraw.xform_widget(se3.identity(),self.attributes.get("length",0.1),self.attributes.get("width",0.01)) self.displayCache[0].draw(drawRaw,transform=item) if name != None: self.drawText(name,se3.apply(item,[-0.05]*3)) elif types == 'IKGoal': if hasattr(item,'robot'): #need this to be built with a robot element. #Otherwise, can't determine the correct transforms robot = item.robot elif world: if world.numRobots() >= 1: robot = world.robot(0) else: robot = None else: robot = None if robot != None: link = robot.link(item.link()) dest = robot.link(item.destLink()) if item.destLink()>=0 else None while len(self.displayCache) < 3: self.displayCache.append(CachedGLObject()) self.displayCache[1].name = self.name+" target position" self.displayCache[2].name = self.name+" curve" if item.numPosDims() != 0: lp,wp = item.getPosition() #set up parameters of connector p1 = se3.apply(link.getTransform(),lp) if dest != None: p2 = se3.apply(dest.getTransform(),wp) else: p2 = wp d = vectorops.distance(p1,p2) v1 = [0.0]*3 v2 = [0.0]*3 if item.numRotDims()==3: #full constraint R = item.getRotation() def drawRaw(): gldraw.xform_widget(se3.identity(),self.attributes.get("length",0.1),self.attributes.get("width",0.01)) t1 = se3.mul(link.getTransform(),(so3.identity(),lp)) t2 = (R,wp) if dest==None else se3.mul(dest.getTransform(),(R,wp)) self.displayCache[0].draw(drawRaw,transform=t1) self.displayCache[1].draw(drawRaw,transform=t2) vlen = d*0.1 v1 = so3.apply(t1[0],[-vlen]*3) v2 = so3.apply(t2[0],[vlen]*3) elif item.numRotDims()==0: #point constraint def drawRaw(): glDisable(GL_LIGHTING) glEnable(GL_POINT_SMOOTH) glPointSize(self.attributes.get("size",5.0)) glColor4f(*self.attributes.get("color",[0,0,0,1])) glBegin(GL_POINTS) glVertex3f(0,0,0) glEnd() self.displayCache[0].draw(drawRaw,transform=(so3.identity(),p1)) self.displayCache[1].draw(drawRaw,transform=(so3.identity(),p2)) #set up the connecting curve vlen = d*0.5 d = vectorops.sub(p2,p1) v1 = vectorops.mul(d,0.5) #curve in the destination v2 = vectorops.cross((0,0,0.5),d) else: #hinge constraint p = [0,0,0] d = [0,0,0] def drawRawLine(): glDisable(GL_LIGHTING) glEnable(GL_POINT_SMOOTH) glPointSize(self.attributes.get("size",5.0)) glColor4f(*self.attributes.get("color",[0,0,0,1])) glBegin(GL_POINTS) glVertex3f(*p) glEnd() glColor4f(*self.attributes.get("color",[0.5,0,0.5,1])) glLineWidth(self.attributes.get("width",3.0)) glBegin(GL_LINES) glVertex3f(*p) glVertex3f(*vectorops.madd(p,d,self.attributes.get("length",0.1))) glEnd() glLineWidth(1.0) ld,wd = item.getRotationAxis() p = lp d = ld self.displayCache[0].draw(drawRawLine,transform=link.getTransform(),parameters=(p,d)) p = wp d = wd self.displayCache[1].draw(drawRawLine,transform=dest.getTransform() if dest else se3.identity(),parameters=(p,d)) #set up the connecting curve d = vectorops.sub(p2,p1) v1 = vectorops.mul(d,0.5) #curve in the destination v2 = vectorops.cross((0,0,0.5),d) def drawConnection(): glDisable(GL_DEPTH_TEST) glDisable(GL_LIGHTING) glColor3f(1,0.5,0) gldraw.hermite_curve(p1,v1,p2,v2,0.03) glEnable(GL_DEPTH_TEST) self.displayCache[2].draw(drawConnection,transform=None,parameters = (p1,v1,p2,v2)) if name != None: self.drawText(name,vectorops.add(wp,[-0.05]*3)) else: wp = link.getTransform()[1] if item.numRotDims()==3: #full constraint R = item.getRotation() def drawRaw(): gldraw.xform_widget(se3.identity(),self.attributes.get("length",0.1),self.attributes.get("width",0.01)) self.displayCache[0].draw(drawRaw,transform=link.getTransform()) self.displayCache[1].draw(drawRaw,transform=se3.mul(link.getTransform(),(R,[0,0,0]))) elif item.numRotDims() > 0: #axis constraint d = [0,0,0] def drawRawLine(): glDisable(GL_LIGHTING) glColor4f(*self.attributes.get("color",[0.5,0,0.5,1])) glLineWidth(self.attributes.get("width",3.0)) glBegin(GL_LINES) glVertex3f(0,0,0) glVertex3f(*vectorops.mul(d,self.attributes.get("length",0.1))) glEnd() glLineWidth(1.0) ld,wd = item.getRotationAxis() d = ld self.displayCache[0].draw(drawRawLine,transform=link.getTransform(),parameters=d) d = wd self.displayCache[1].draw(drawRawLine,transform=(dest.getTransform()[0] if dest else so3.identity(),wp),parameters=d) else: #no drawing pass if name != None: self.drawText(name,se3.apply(wp,[-0.05]*3)) else: print "Unable to draw item of type",types #revert appearance if not self.useDefaultAppearance and hasattr(item,'appearance'): item.appearance().set(self.oldAppearance)
def integrate(self, x, d): """For Lie groups, returns the point that would be arrived at via integrating the difference vector d starting from x. Must satisfy the relationship a = integrate(b,difference(a,b)). In Cartesian spaces it is x+d""" return vectorops.add(x, d)
def apply(T, point): """Applies the transform T to the given point""" return vectorops.add(apply_rotation(T, point), T[1])
def apply(T,point): """Applies the transform T to the given point""" return vectorops.add(apply_rotation(T,point),T[1])
def integrate(self,x,d): """For Lie groups, returns the point that would be arrived at via integrating the difference vector d starting from x. Must satisfy the relationship a = integrate(b,difference(a,b)). In Cartesian spaces it is x+d""" return vectorops.add(x,d)
def localOffset(self,dir): """Offsets this direction by a vector in local coordinates""" self._localCoordinates = vectorops.add(self._localCoordinates,dir)