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 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 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)
