def prepare_GL(self):
GLProgram.prepare_GL(self)
# Projection
glMatrixMode(GL_PROJECTION)
glLoadIdentity()
gluPerspective (self.fov,float(self.width)/float(self.height),self.clippingplanes[0],self.clippingplanes[1])
# Initialize ModelView matrix
glMatrixMode(GL_MODELVIEW)
glLoadIdentity()
# View transformation
mat = se3.homogeneous(self.camera.matrix())
cols = zip(*mat)
pack = sum((list(c) for c in cols),[])
glMultMatrixf(pack)
# Light source
glLightfv(GL_LIGHT0,GL_POSITION,[0,-1,2,0])
glLightfv(GL_LIGHT0,GL_DIFFUSE,[1,1,1,1])
glLightfv(GL_LIGHT0,GL_SPECULAR,[1,1,1,1])
glEnable(GL_LIGHT0)
glLightfv(GL_LIGHT1,GL_POSITION,[-1,2,1,0])
glLightfv(GL_LIGHT1,GL_DIFFUSE,[0.5,0.5,0.5,1])
glLightfv(GL_LIGHT1,GL_SPECULAR,[0.5,0.5,0.5,1])
glEnable(GL_LIGHT1)
def prepare_GL(self):
GLProgram.prepare_GL(self)
# Projection
glMatrixMode(GL_PROJECTION)
glLoadIdentity()
gluPerspective (self.fov*float(self.height)/float(self.width),float(self.width)/float(self.height),self.clippingplanes[0],self.clippingplanes[1])
# Initialize ModelView matrix
glMatrixMode(GL_MODELVIEW)
glLoadIdentity()
# View transformation
mat = se3.homogeneous(self.camera.matrix())
cols = zip(*mat)
pack = sum((list(c) for c in cols),[])
glMultMatrixf(pack)
# Light source
glLightfv(GL_LIGHT0,GL_POSITION,[0,-1,2,0])
glLightfv(GL_LIGHT0,GL_DIFFUSE,[1,1,1,1])
glLightfv(GL_LIGHT0,GL_SPECULAR,[1,1,1,1])
glEnable(GL_LIGHT0)
glLightfv(GL_LIGHT1,GL_POSITION,[-1,2,1,0])
glLightfv(GL_LIGHT1,GL_DIFFUSE,[0.5,0.5,0.5,1])
glLightfv(GL_LIGHT1,GL_SPECULAR,[0.5,0.5,0.5,1])
glEnable(GL_LIGHT1)
def xform_widget(T, length, width, lighting=True, fancy=False):
"""Draws an axis-aligned transform widget for the se3 transform T.
Length / width govern the length / width of the axes. If fancy=True,
draws the axes with real volume rather than lines"""
mat = zip(*se3.homogeneous(T))
mat = sum([list(coli) for coli in mat], [])
glPushMatrix()
glMultMatrixf(mat)
if fancy:
#center
setcolor(1, 1, 1, 1, lighting=lighting)
box((-width * 0.75, -width * 0.75, -width * 0.75),
(width * 0.75, width * 0.75, width * 0.75),
lighting=lighting)
#x axis
setcolor(1, 0, 0, 1, lighting=lighting)
box((-width * 0.5, -width * 0.5, -width * 0.5),
(length, width * 0.5, width * 0.5),
lighting=lighting)
#y axis
setcolor(0, 1, 0, 1, lighting=lighting)
box((-width * 0.5, -width * 0.5, -width * 0.5),
(width * 0.5, length, width * 0.5),
lighting=lighting)
#z axis
setcolor(0, 0, 1, 1, lighting=lighting)
box((-width * 0.5, -width * 0.5, -width * 0.5),
(width * 0.5, width * 0.5, length),
lighting=lighting)
else:
glDisable(GL_LIGHTING)
glBegin(GL_LINES)
glColor4f(1, 1, 1, 1)
glVertex3f(0, 0, 0)
glColor4f(1, 0, 0, 1)
glVertex3f(length, 0, 0)
glColor4f(1, 1, 1, 1)
glVertex3f(0, 0, 0)
glColor4f(0, 1, 0, 1)
glVertex3f(0, length, 0)
glColor4f(1, 1, 1, 1)
glVertex3f(0, 0, 0)
glColor4f(0, 0, 1, 1)
glVertex3f(0, 0, length)
glEnd()
glPopMatrix()
def draw(self,renderFunction,transform=None,parameters=None):
"""Given the function that actually makes OpenGL calls, this
will draw the object.
If parameters is given, the object's local appearance is assumed
to be defined deterministically from these parameters. The display
list will be redrawn if the parameters change.
"""
if self.makingDisplayList:
renderFunction()
return
if self.glDisplayList == None or self.changed or parameters != self.displayListParameters:
self.displayListParameters = parameters
self.changed = False
if self.glDisplayList == None:
print "Generating new display list",self.name
self.glDisplayList = glGenLists(1)
print "Compiling display list",self.name
if transform:
glPushMatrix()
glMultMatrixf(sum(zip(*se3.homogeneous(transform)),()))
glNewList(self.glDisplayList,GL_COMPILE_AND_EXECUTE)
self.makingDisplayList = True
renderFunction()
self.makingDisplayList = False
glEndList()
if transform:
glPopMatrix()
else:
if transform:
glPushMatrix()
glMultMatrixf(sum(zip(*se3.homogeneous(transform)),()))
glCallList(self.glDisplayList)
if transform:
glPopMatrix()
def xform_widget(T,length,width,lighting=True,fancy=False):
"""Draws an axis-aligned transform widget for the se3 transform T.
Length / width govern the length / width of the axes. If fancy=True,
draws the axes with real volume rather than lines"""
mat = zip(*se3.homogeneous(T))
mat = sum([list(coli) for coli in mat],[])
glPushMatrix()
glMultMatrixf(mat)
if fancy:
#center
setcolor(1,1,1,1,lighting=lighting)
box((-width*0.75,-width*0.75,-width*0.75),(width*0.75,width*0.75,width*0.75),lighting=lighting)
#x axis
setcolor(1,0,0,1,lighting=lighting)
box((-width*0.5,-width*0.5,-width*0.5),(length,width*0.5,width*0.5),lighting=lighting)
#y axis
setcolor(0,1,0,1,lighting=lighting)
box((-width*0.5,-width*0.5,-width*0.5),(width*0.5,length,width*0.5),lighting=lighting)
#z axis
setcolor(0,0,1,1,lighting=lighting)
box((-width*0.5,-width*0.5,-width*0.5),(width*0.5,width*0.5,length),lighting=lighting)
else:
glDisable(GL_LIGHTING)
glBegin(GL_LINES)
glColor4f(1,1,1,1)
glVertex3f(0,0,0)
glColor4f(1,0,0,1)
glVertex3f(length,0,0)
glColor4f(1,1,1,1)
glVertex3f(0,0,0)
glColor4f(0,1,0,1)
glVertex3f(0,length,0)
glColor4f(1,1,1,1)
glVertex3f(0,0,0)
glColor4f(0,0,1,1)
glVertex3f(0,0,length)
glEnd()
glPopMatrix()
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)