parameters: (self,world=None,viewport=None,draw_transparent=None)
docstring: Draws the specified item in the specified world, with all the
          current modifications in attributes.
  
          If a name or label are given, and self.attributes['hide_label'] != False, then the
          label is shown.
  
          If draw_transparent is None, then everything is drawn.  If True, then
          only transparent items are drawn.  If False, then only opaque items are
          drawn.  (This only affects WorldModels)
docstring_summary: Draws the specified item in the specified world, with all the
          current modifications in attributes.
docstring_tokens: Draws the specified item in the specified world with all the current modifications in attributes .
function: def draw(self,world=None,viewport=None,draw_transparent=None):
          """Draws the specified item in the specified world, with all the
          current modifications in attributes.
  
          If a name or label are given, and self.attributes['hide_label'] != False, then the
          label is shown.
  
          If draw_transparent is None, then everything is drawn.  If True, then
          only transparent items are drawn.  If False, then only opaque items are
          drawn.  (This only affects WorldModels)
          """
          if self.attributes["hidden"]:
              return
          if self.customDrawFunc is not None:
              self.customDrawFunc(self.item)
              return
         
          item = self.item
          name = None
          if not self.attributes["hide_label"]:
              name = self.attributes['label']
          #set appearance
          if not self.useDefaultAppearance and hasattr(item,'appearance'):
              #print "Has custom appearance"
              if not hasattr(self,'oldAppearance'):
                  self.oldAppearance = item.appearance().clone()
              if self.customAppearance is not None:
                  item.appearance().set(self.customAppearance)
              elif "color" in self.attributes:
                  item.appearance().setColor(*self.attributes["color"])
  
          if self.editor is not None:
              assert viewport is not None
              #some weird logic to determine whether to draw now...
              if draw_transparent is None or draw_transparent == self.transparent() or (self.transparent() is None and draw_transparent==False):
                  #KLUDGE:
                  #might be a robot, make sure the appearances are all up to date before drawing the editor
                  for n,app in self.subAppearances.iteritems():
                      if app.useDefaultAppearance or not hasattr(app.item,'appearance'):
                          continue
                      if not hasattr(app,'oldAppearance'):
                          app.oldAppearance = app.item.appearance().clone()
                      if app.customAppearance is not None:
                          app.item.appearance().set(app.customAppearance)
                      elif "color" in app.attributes:
                          app.item.appearance().setColor(*app.attributes["color"])
  
                  self.editor.drawGL(viewport)
  
                  #Restore sub-appearances
                  for n,app in self.subAppearances.iteritems():
                      if app.useDefaultAppearance or not hasattr(app.item,'appearance'):
                          continue
                      app.item.appearance().set(app.oldAppearance)
              if isinstance(self.editor,RobotPoser):
                  #the widget took care of everything, dont continue drawing the item
                  #revert appearance if necessary
                  if not self.useDefaultAppearance and hasattr(item,'appearance'):
                      item.appearance().set(self.oldAppearance)
                  return
  
          if len(self.subAppearances)!=0:
              for n,app in self.subAppearances.iteritems():
                  if draw_transparent is True:
                      if not app.transparent():
                          continue
                  elif draw_transparent is False:
                      if app.transparent():
                          continue
                  app.widget = self.widget
                  app.draw(world,viewport,draw_transparent)
          elif hasattr(item,'drawGL'):
              item.drawGL()
          elif hasattr(item,'drawWorldGL'):
              item.drawWorldGL()
          elif isinstance(item,str):
              pos = self.attributes["position"]
              if pos is not None and len(pos)==3:
                  col = self.attributes["color"]
                  self.widget.addLabel(self.item,pos,col)
          elif isinstance(item,VisPlot):
              pass
          elif isinstance(item,Trajectory):
              doDraw = False
              centroid = None
              robot = (world.robot(self.attributes["robot"]) if world is not None and world.numRobots() > 0 else None)
              if robot is not None:
                  robotConfig = robot.getConfig()
              treatAsRobotTrajectory = (item.__class__ == Trajectory and len(item.milestones) > 0 and robot and len(item.milestones[0]) == robot.numLinks())
              if isinstance(item,RobotTrajectory) or treatAsRobotTrajectory:
                  ees = self.attributes.get("endeffectors",[-1])
                  if world:
                      doDraw = (len(ees) > 0)
                      for i,ee in enumerate(ees):
                          if ee < 0: ees[i] = robot.numLinks()-1
                      if doDraw:
                          robot.setConfig(item.milestones[0])
                          centroid = vectorops.div(vectorops.add(*[robot.link(ee).getTransform()[1] for ee in ees]),len(ees))
              elif isinstance(item,SE3Trajectory):
                  doDraw = True
                  centroid = item.milestones[0][9:]
              else:
                  if len(item.milestones[0]) == 3:
                      #R3 trajectory
                      doDraw = True
                      centroid = item.milestones[0]
                  elif len(item.milestones[0]) == 2:
                      #R2 trajectory
                      doDraw = True
                      centroid = item.milestones[0]+[0.0]
                  else:
                      #don't know how to interpret this trajectory
                      pass
              if doDraw:
                  def drawRaw():
                      pointTrajectories = []
                      width = self.attributes["width"]
                      color = self.attributes["color"]
                      pointSize = self.attributes["pointSize"]
                      pointColor = self.attributes["pointColor"]
                      if isinstance(item,RobotTrajectory) or treatAsRobotTrajectory:
                          ees = self.attributes.get("endeffectors",[-1])
                          drawRobotTrajectory(item,robot,ees,width,color,pointSize,pointColor) 
                      else:
                          drawTrajectory(item,width,color,pointSize,pointColor)
                  self.displayCache[0].draw(drawRaw)
                  if name is not None:
                      self.drawText(name,centroid)
                  if robot is not None:
                      robot.setConfig(robotConfig)
          elif isinstance(item,MultiPath):
              robot = (world.robot(self.attributes["robot"]) if world is not None and world.numRobots() > 0 else None)
              if robot is not None and item.numSections() > 0:
                  if len(item.sections[0].configs[0]) == robot.numLinks():
                      ees = self.attributes.get("endeffectors",[-1])
                      if len(ees) > 0:
                          for i,ee in enumerate(ees):
                              if ee < 0: ees[i] = robot.numLinks()-1
                          robot.setConfig(item.sections[0].configs[0])
                          centroid = vectorops.div(vectorops.add(*[robot.link(ee).getTransform()[1] for ee in ees]),len(ees))
                      width = self.attributes["width"]
                      color = self.attributes["color"]
                      pointSize = self.attributes["pointSize"]
                      pointColor = self.attributes["pointColor"]
                      color2 = [1-c for c in color]
                      color2[3] = color[3]
                      def drawRaw():
                          for i,s in enumerate(item.sections):
                              drawRobotTrajectory(s.configs,robot,ees,width,(color if i%2 == 0 else color2),pointSize,pointColor)
                      #draw it!
                      self.displayCache[0].draw(drawRaw)
                      if name is not None:
                          self.drawText(name,centroid)
          elif isinstance(item,coordinates.Point):
              def drawRaw():
                  glDisable(GL_LIGHTING)
                  glEnable(GL_POINT_SMOOTH)
                  glPointSize(self.attributes["size"])
                  glColor4f(*self.attributes["color"])
                  glBegin(GL_POINTS)
                  glVertex3f(0,0,0)
                  glEnd()
                  #write name
              glDisable(GL_DEPTH_TEST)
              self.displayCache[0].draw(drawRaw,[so3.identity(),item.worldCoordinates()])
              glEnable(GL_DEPTH_TEST)
              if name is not None:
                  self.drawText(name,item.worldCoordinates())
          elif isinstance(item,coordinates.Direction):
              def drawRaw():
                  glDisable(GL_LIGHTING)
                  glDisable(GL_DEPTH_TEST)
                  L = self.attributes["length"]
                  source = [0,0,0]
                  glColor4f(*self.attributes["color"])
                  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 is not None:
                  self.drawText(name,vectorops.add(item.frame().worldCoordinates()[1],item.worldCoordinates()))
          elif isinstance(item,coordinates.Frame):
              t = item.worldCoordinates()
              if item.parent() is not 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["length"],self.attributes["width"])
                  glLineWidth(1.0)
                  #draw curve between frame and parent
                  if item.parent() is not 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*max(0.1,vectorops.norm(tlocal[1])))
                      #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 is not None:
                  self.drawText(name,t[1])
          elif isinstance(item,coordinates.Transform):
              #draw curve between frames
              t1 = item.source().worldCoordinates()
              if item.destination() is not 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 is not None:
                  self.drawText(name,spline.hermite_eval(t1[1],v1,t2[1],v2,0.5))
          elif isinstance(item,coordinates.Group):
              pass
          elif isinstance(item,ContactPoint):
              def drawRaw():
                  glDisable(GL_LIGHTING)
                  glEnable(GL_POINT_SMOOTH)
                  glPointSize(self.attributes["size"])
                  l = self.attributes["length"]
                  glColor4f(*self.attributes["color"])
                  glBegin(GL_POINTS)
                  glVertex3f(0,0,0)
                  glEnd()
                  glBegin(GL_LINES)
                  glVertex3f(0,0,0)
                  glVertex3f(l,0,0)
                  glEnd()
              self.displayCache[0].draw(drawRaw,[so3.canonical(item.n),item.x])
          elif isinstance(item,Hold):
              pass
          elif isinstance(item,IKObjective):
              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 is not None and world.numRobots() > 0:
                      robot = world.robot(self.attributes.get("robot",0))
                  else:
                      robot = None
              else:
                  robot = None
              if robot is not 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(glcommon.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 is not 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["length"],self.attributes["width"])
                          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["size"])
                              glColor4f(*self.attributes["color"])
                              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["size"])
                              glColor4f(*self.attributes["color"])
                              glBegin(GL_POINTS)
                              glVertex3f(*p)
                              glEnd()
                              glColor4f(*self.attributes["color"])
                              glLineWidth(self.attributes["width"])
                              glBegin(GL_LINES)
                              glVertex3f(*p)
                              glVertex3f(*vectorops.madd(p,d,self.attributes["length"]))
                              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_LIGHTING)
                          glDisable(GL_DEPTH_TEST)
                          glColor3f(1,0.5,0)
                          gldraw.hermite_curve(p1,v1,p2,v2,0.03*max(0.1,vectorops.distance(p1,p2)))
                          #glBegin(GL_LINES)
                          #glVertex3f(*p1)
                          #glVertex3f(*p2)
                          #glEnd()
                          glEnable(GL_DEPTH_TEST)
                      #TEMP for some reason the cached version sometimes gives a GL error
                      self.displayCache[2].draw(drawConnection,transform=None,parameters = (p1,v1,p2,v2))
                      #drawConnection()
                      if name is not None:
                          self.drawText(name,wp)
                  else:
                      wp = link.getTransform()[1]
                      if item.numRotDims()==3: #full constraint
                          R = item.getRotation()
                          def drawRaw():
                              gldraw.xform_widget(se3.identity(),self.attributes["length"],self.attributes["width"])
                          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["axis_color"])
                              glLineWidth(self.attributes["axis_width"])
                              glBegin(GL_LINES)
                              glVertex3f(0,0,0)
                              glVertex3f(*vectorops.mul(d,self.attributes["axis_length"]))
                              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 is not None:
                          self.drawText(name,wp)
          elif isinstance(item,(GeometricPrimitive,TriangleMesh,PointCloud,Geometry3D)):
              #this can be tricky if the mesh or point cloud has colors
              if not hasattr(self,'appearance'):
                  self.appearance = Appearance()
                  self.appearance.setColor(0.5,0.5,0.5,1)
              c = self.attributes["color"]
              if c is not None:
                  self.appearance.setColor(*c)
              s = self.attributes["size"]
              if s:
                  self.appearance.setPointSize(s)
              wp = None
              geometry = None
              lighting = True
              if isinstance(self.item,GeometricPrimitive):
                  if not hasattr(self,'geometry'):
                      self.geometry = Geometry3D(self.item)
                  geometry = self.geometry
                  if self.item.type not in ['Sphere','AABB']:
                      lighting = False
              elif isinstance(self.item,PointCloud):
                  if not hasattr(self,'geometry'):
                      self.geometry = Geometry3D(self.item)
                  lighting = False
                  geometry = self.geometry
              elif isinstance(self.item,TriangleMesh):
                  if not hasattr(self,'geometry'):
                      self.geometry = Geometry3D(self.item)
                  geometry = self.geometry
              else:
                  assert isinstance(self.item,Geometry3D)
                  if self.item.type() == 'GeometricPrimitive':
                      prim = self.item.getGeometricPrimitive()
                      if prim.type not in ['Sphere','AABB']:
                          lighting = False
                  elif self.item.type() == 'PointCloud':
                      lighting = False
                  geometry = self.item
              if lighting:
                  glEnable(GL_LIGHTING)
              else:
                  glDisable(GL_LIGHTING)
              self.appearance.drawWorldGL(geometry)
              if name is not None:
                  bmin,bmax = geometry.getBB()
                  wp = vectorops.mul(vectorops.add(bmin,bmax),0.5)
                  self.drawText(name,wp)
          else:
              try:
                  itypes = self.attributes['type']
              except KeyError:
                  try:
                      itypes = objectToVisType(item,world)
                  except Exception as e:
                      import traceback
                      traceback.print_exc()
                      print e
                      print "visualization.py: Unsupported object type",item,"of type:",item.__class__.__name__
                      return
              if itypes is None:
                  print "Unable to convert item",item,"to drawable"
                  return
              elif itypes == 'Config':
                  if world and world.numRobots() >= 1:
                      robot = world.robot(self.attributes.get("robot",0))
                      if not self.useDefaultAppearance:
                          oldAppearance = [robot.link(i).appearance().clone() for i in xrange(robot.numLinks())]
                          for i in xrange(robot.numLinks()):
                              if self.customAppearance is not None:
                                  robot.link(i).appearance().set(self.customAppearance)
                              elif "color" in self.attributes:
                                  robot.link(i).appearance().setColor(*self.attributes["color"])
  
                      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 items without a world or robot"
              elif itypes == 'Configs':
                  if world and world.numRobots() >= 1:
                      maxConfigs = self.attributes.get("maxConfigs",min(10,len(item)))
                      robot = world.robot(self.attributes.get("robot",0))
                      if not self.useDefaultAppearance:
                          oldAppearance = [robot.link(i).appearance().clone() for i in xrange(robot.numLinks())]
                          for i in xrange(robot.numLinks()):
                            if self.customAppearance is not None:
                              robot.link(i).appearance().set(self.customAppearance)
                            elif "color" in self.attributes:
                              robot.link(i).appearance().setColor(*self.attributes["color"])
  
                      oldconfig = robot.getConfig()
                      for i in range(maxConfigs):
                          idx = int(i*len(item))//maxConfigs
                          robot.setConfig(item[idx])
                          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 Configs items without a world or robot"
              elif itypes == '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 is not None:
                      self.drawText(name,item)
              elif itypes == 'RigidTransform':
                  def drawRaw():
                      fancy = self.attributes.get("fancy",False)
                      if fancy: glEnable(GL_LIGHTING)
                      else: glDisable(GL_LIGHTING)
                      gldraw.xform_widget(se3.identity(),self.attributes.get("length",0.1),self.attributes.get("width",0.01),fancy=fancy)
                  self.displayCache[0].draw(drawRaw,transform=item)
                  if name is not None:
                      self.drawText(name,item[1])
              else:
                  print "Unable to draw item of type \"%s\""%(str(itypes),)
  
          #revert appearance
          if not self.useDefaultAppearance and hasattr(item,'appearance'):
              item.appearance().set(self.oldAppearance)
function_tokens: def draw ( self , world = None , viewport = None , draw_transparent = None ) : if self . attributes [ "hidden" ] : return if self . customDrawFunc is not None : self . customDrawFunc ( self . item ) return item = self . item name = None if not self . attributes [ "hide_label" ] : name = self . attributes [ 'label' ] #set appearance if not self . useDefaultAppearance and hasattr ( item , 'appearance' ) : #print "Has custom appearance" if not hasattr ( self , 'oldAppearance' ) : self . oldAppearance = item . appearance ( ) . clone ( ) if self . customAppearance is not None : item . appearance ( ) . set ( self . customAppearance ) elif "color" in self . attributes : item . appearance ( ) . setColor ( * self . attributes [ "color" ] ) if self . editor is not None : assert viewport is not None #some weird logic to determine whether to draw now... if draw_transparent is None or draw_transparent == self . transparent ( ) or ( self . transparent ( ) is None and draw_transparent == False ) : #KLUDGE: #might be a robot, make sure the appearances are all up to date before drawing the editor for n , app in self . subAppearances . iteritems ( ) : if app . useDefaultAppearance or not hasattr ( app . item , 'appearance' ) : continue if not hasattr ( app , 'oldAppearance' ) : app . oldAppearance = app . item . appearance ( ) . clone ( ) if app . customAppearance is not None : app . item . appearance ( ) . set ( app . customAppearance ) elif "color" in app . attributes : app . item . appearance ( ) . setColor ( * app . attributes [ "color" ] ) self . editor . drawGL ( viewport ) #Restore sub-appearances for n , app in self . subAppearances . iteritems ( ) : if app . useDefaultAppearance or not hasattr ( app . item , 'appearance' ) : continue app . item . appearance ( ) . set ( app . oldAppearance ) if isinstance ( self . editor , RobotPoser ) : #the widget took care of everything, dont continue drawing the item #revert appearance if necessary if not self . useDefaultAppearance and hasattr ( item , 'appearance' ) : item . appearance ( ) . set ( self . oldAppearance ) return if len ( self . subAppearances ) != 0 : for n , app in self . subAppearances . iteritems ( ) : if draw_transparent is True : if not app . transparent ( ) : continue elif draw_transparent is False : if app . transparent ( ) : continue app . widget = self . widget app . draw ( world , viewport , draw_transparent ) elif hasattr ( item , 'drawGL' ) : item . drawGL ( ) elif hasattr ( item , 'drawWorldGL' ) : item . drawWorldGL ( ) elif isinstance ( item , str ) : pos = self . attributes [ "position" ] if pos is not None and len ( pos ) == 3 : col = self . attributes [ "color" ] self . widget . addLabel ( self . item , pos , col ) elif isinstance ( item , VisPlot ) : pass elif isinstance ( item , Trajectory ) : doDraw = False centroid = None robot = ( world . robot ( self . attributes [ "robot" ] ) if world is not None and world . numRobots ( ) > 0 else None ) if robot is not None : robotConfig = robot . getConfig ( ) treatAsRobotTrajectory = ( item . __class__ == Trajectory and len ( item . milestones ) > 0 and robot and len ( item . milestones [ 0 ] ) == robot . numLinks ( ) ) if isinstance ( item , RobotTrajectory ) or treatAsRobotTrajectory : ees = self . attributes . get ( "endeffectors" , [ - 1 ] ) if world : doDraw = ( len ( ees ) > 0 ) for i , ee in enumerate ( ees ) : if ee < 0 : ees [ i ] = robot . numLinks ( ) - 1 if doDraw : robot . setConfig ( item . milestones [ 0 ] ) centroid = vectorops . div ( vectorops . add ( * [ robot . link ( ee ) . getTransform ( ) [ 1 ] for ee in ees ] ) , len ( ees ) ) elif isinstance ( item , SE3Trajectory ) : doDraw = True centroid = item . milestones [ 0 ] [ 9 : ] else : if len ( item . milestones [ 0 ] ) == 3 : #R3 trajectory doDraw = True centroid = item . milestones [ 0 ] elif len ( item . milestones [ 0 ] ) == 2 : #R2 trajectory doDraw = True centroid = item . milestones [ 0 ] + [ 0.0 ] else : #don't know how to interpret this trajectory pass if doDraw : def drawRaw ( ) : pointTrajectories = [ ] width = self . attributes [ "width" ] color = self . attributes [ "color" ] pointSize = self . attributes [ "pointSize" ] pointColor = self . attributes [ "pointColor" ] if isinstance ( item , RobotTrajectory ) or treatAsRobotTrajectory : ees = self . attributes . get ( "endeffectors" , [ - 1 ] ) drawRobotTrajectory ( item , robot , ees , width , color , pointSize , pointColor ) else : drawTrajectory ( item , width , color , pointSize , pointColor ) self . displayCache [ 0 ] . draw ( drawRaw ) if name is not None : self . drawText ( name , centroid ) if robot is not None : robot . setConfig ( robotConfig ) elif isinstance ( item , MultiPath ) : robot = ( world . robot ( self . attributes [ "robot" ] ) if world is not None and world . numRobots ( ) > 0 else None ) if robot is not None and item . numSections ( ) > 0 : if len ( item . sections [ 0 ] . configs [ 0 ] ) == robot . numLinks ( ) : ees = self . attributes . get ( "endeffectors" , [ - 1 ] ) if len ( ees ) > 0 : for i , ee in enumerate ( ees ) : if ee < 0 : ees [ i ] = robot . numLinks ( ) - 1 robot . setConfig ( item . sections [ 0 ] . configs [ 0 ] ) centroid = vectorops . div ( vectorops . add ( * [ robot . link ( ee ) . getTransform ( ) [ 1 ] for ee in ees ] ) , len ( ees ) ) width = self . attributes [ "width" ] color = self . attributes [ "color" ] pointSize = self . attributes [ "pointSize" ] pointColor = self . attributes [ "pointColor" ] color2 = [ 1 - c for c in color ] color2 [ 3 ] = color [ 3 ] def drawRaw ( ) : for i , s in enumerate ( item . sections ) : drawRobotTrajectory ( s . configs , robot , ees , width , ( color if i % 2 == 0 else color2 ) , pointSize , pointColor ) #draw it! self . displayCache [ 0 ] . draw ( drawRaw ) if name is not None : self . drawText ( name , centroid ) elif isinstance ( item , coordinates . Point ) : def drawRaw ( ) : glDisable ( GL_LIGHTING ) glEnable ( GL_POINT_SMOOTH ) glPointSize ( self . attributes [ "size" ] ) glColor4f ( * self . attributes [ "color" ] ) glBegin ( GL_POINTS ) glVertex3f ( 0 , 0 , 0 ) glEnd ( ) #write name glDisable ( GL_DEPTH_TEST ) self . displayCache [ 0 ] . draw ( drawRaw , [ so3 . identity ( ) , item . worldCoordinates ( ) ] ) glEnable ( GL_DEPTH_TEST ) if name is not None : self . drawText ( name , item . worldCoordinates ( ) ) elif isinstance ( item , coordinates . Direction ) : def drawRaw ( ) : glDisable ( GL_LIGHTING ) glDisable ( GL_DEPTH_TEST ) L = self . attributes [ "length" ] source = [ 0 , 0 , 0 ] glColor4f ( * self . attributes [ "color" ] ) 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 is not None : self . drawText ( name , vectorops . add ( item . frame ( ) . worldCoordinates ( ) [ 1 ] , item . worldCoordinates ( ) ) ) elif isinstance ( item , coordinates . Frame ) : t = item . worldCoordinates ( ) if item . parent ( ) is not 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 [ "length" ] , self . attributes [ "width" ] ) glLineWidth ( 1.0 ) #draw curve between frame and parent if item . parent ( ) is not 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 * max ( 0.1 , vectorops . norm ( tlocal [ 1 ] ) ) ) #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 is not None : self . drawText ( name , t [ 1 ] ) elif isinstance ( item , coordinates . Transform ) : #draw curve between frames t1 = item . source ( ) . worldCoordinates ( ) if item . destination ( ) is not 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 is not None : self . drawText ( name , spline . hermite_eval ( t1 [ 1 ] , v1 , t2 [ 1 ] , v2 , 0.5 ) ) elif isinstance ( item , coordinates . Group ) : pass elif isinstance ( item , ContactPoint ) : def drawRaw ( ) : glDisable ( GL_LIGHTING ) glEnable ( GL_POINT_SMOOTH ) glPointSize ( self . attributes [ "size" ] ) l = self . attributes [ "length" ] glColor4f ( * self . attributes [ "color" ] ) glBegin ( GL_POINTS ) glVertex3f ( 0 , 0 , 0 ) glEnd ( ) glBegin ( GL_LINES ) glVertex3f ( 0 , 0 , 0 ) glVertex3f ( l , 0 , 0 ) glEnd ( ) self . displayCache [ 0 ] . draw ( drawRaw , [ so3 . canonical ( item . n ) , item . x ] ) elif isinstance ( item , Hold ) : pass elif isinstance ( item , IKObjective ) : 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 is not None and world . numRobots ( ) > 0 : robot = world . robot ( self . attributes . get ( "robot" , 0 ) ) else : robot = None else : robot = None if robot is not 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 ( glcommon . 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 is not 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 [ "length" ] , self . attributes [ "width" ] ) 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 [ "size" ] ) glColor4f ( * self . attributes [ "color" ] ) 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 [ "size" ] ) glColor4f ( * self . attributes [ "color" ] ) glBegin ( GL_POINTS ) glVertex3f ( * p ) glEnd ( ) glColor4f ( * self . attributes [ "color" ] ) glLineWidth ( self . attributes [ "width" ] ) glBegin ( GL_LINES ) glVertex3f ( * p ) glVertex3f ( * vectorops . madd ( p , d , self . attributes [ "length" ] ) ) 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_LIGHTING ) glDisable ( GL_DEPTH_TEST ) glColor3f ( 1 , 0.5 , 0 ) gldraw . hermite_curve ( p1 , v1 , p2 , v2 , 0.03 * max ( 0.1 , vectorops . distance ( p1 , p2 ) ) ) #glBegin(GL_LINES) #glVertex3f(*p1) #glVertex3f(*p2) #glEnd() glEnable ( GL_DEPTH_TEST ) #TEMP for some reason the cached version sometimes gives a GL error self . displayCache [ 2 ] . draw ( drawConnection , transform = None , parameters = ( p1 , v1 , p2 , v2 ) ) #drawConnection() if name is not None : self . drawText ( name , wp ) else : wp = link . getTransform ( ) [ 1 ] if item . numRotDims ( ) == 3 : #full constraint R = item . getRotation ( ) def drawRaw ( ) : gldraw . xform_widget ( se3 . identity ( ) , self . attributes [ "length" ] , self . attributes [ "width" ] ) 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 [ "axis_color" ] ) glLineWidth ( self . attributes [ "axis_width" ] ) glBegin ( GL_LINES ) glVertex3f ( 0 , 0 , 0 ) glVertex3f ( * vectorops . mul ( d , self . attributes [ "axis_length" ] ) ) 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 is not None : self . drawText ( name , wp ) elif isinstance ( item , ( GeometricPrimitive , TriangleMesh , PointCloud , Geometry3D ) ) : #this can be tricky if the mesh or point cloud has colors if not hasattr ( self , 'appearance' ) : self . appearance = Appearance ( ) self . appearance . setColor ( 0.5 , 0.5 , 0.5 , 1 ) c = self . attributes [ "color" ] if c is not None : self . appearance . setColor ( * c ) s = self . attributes [ "size" ] if s : self . appearance . setPointSize ( s ) wp = None geometry = None lighting = True if isinstance ( self . item , GeometricPrimitive ) : if not hasattr ( self , 'geometry' ) : self . geometry = Geometry3D ( self . item ) geometry = self . geometry if self . item . type not in [ 'Sphere' , 'AABB' ] : lighting = False elif isinstance ( self . item , PointCloud ) : if not hasattr ( self , 'geometry' ) : self . geometry = Geometry3D ( self . item ) lighting = False geometry = self . geometry elif isinstance ( self . item , TriangleMesh ) : if not hasattr ( self , 'geometry' ) : self . geometry = Geometry3D ( self . item ) geometry = self . geometry else : assert isinstance ( self . item , Geometry3D ) if self . item . type ( ) == 'GeometricPrimitive' : prim = self . item . getGeometricPrimitive ( ) if prim . type not in [ 'Sphere' , 'AABB' ] : lighting = False elif self . item . type ( ) == 'PointCloud' : lighting = False geometry = self . item if lighting : glEnable ( GL_LIGHTING ) else : glDisable ( GL_LIGHTING ) self . appearance . drawWorldGL ( geometry ) if name is not None : bmin , bmax = geometry . getBB ( ) wp = vectorops . mul ( vectorops . add ( bmin , bmax ) , 0.5 ) self . drawText ( name , wp ) else : try : itypes = self . attributes [ 'type' ] except KeyError : try : itypes = objectToVisType ( item , world ) except Exception as e : import traceback traceback . print_exc ( ) print e print "visualization.py: Unsupported object type" , item , "of type:" , item . __class__ . __name__ return if itypes is None : print "Unable to convert item" , item , "to drawable" return elif itypes == 'Config' : if world and world . numRobots ( ) >= 1 : robot = world . robot ( self . attributes . get ( "robot" , 0 ) ) if not self . useDefaultAppearance : oldAppearance = [ robot . link ( i ) . appearance ( ) . clone ( ) for i in xrange ( robot . numLinks ( ) ) ] for i in xrange ( robot . numLinks ( ) ) : if self . customAppearance is not None : robot . link ( i ) . appearance ( ) . set ( self . customAppearance ) elif "color" in self . attributes : robot . link ( i ) . appearance ( ) . setColor ( * self . attributes [ "color" ] ) 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 items without a world or robot" elif itypes == 'Configs' : if world and world . numRobots ( ) >= 1 : maxConfigs = self . attributes . get ( "maxConfigs" , min ( 10 , len ( item ) ) ) robot = world . robot ( self . attributes . get ( "robot" , 0 ) ) if not self . useDefaultAppearance : oldAppearance = [ robot . link ( i ) . appearance ( ) . clone ( ) for i in xrange ( robot . numLinks ( ) ) ] for i in xrange ( robot . numLinks ( ) ) : if self . customAppearance is not None : robot . link ( i ) . appearance ( ) . set ( self . customAppearance ) elif "color" in self . attributes : robot . link ( i ) . appearance ( ) . setColor ( * self . attributes [ "color" ] ) oldconfig = robot . getConfig ( ) for i in range ( maxConfigs ) : idx = int ( i * len ( item ) ) // maxConfigs robot . setConfig ( item [ idx ] ) 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 Configs items without a world or robot" elif itypes == '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 is not None : self . drawText ( name , item ) elif itypes == 'RigidTransform' : def drawRaw ( ) : fancy = self . attributes . get ( "fancy" , False ) if fancy : glEnable ( GL_LIGHTING ) else : glDisable ( GL_LIGHTING ) gldraw . xform_widget ( se3 . identity ( ) , self . attributes . get ( "length" , 0.1 ) , self . attributes . get ( "width" , 0.01 ) , fancy = fancy ) self . displayCache [ 0 ] . draw ( drawRaw , transform = item ) if name is not None : self . drawText ( name , item [ 1 ] ) else : print "Unable to draw item of type \"%s\"" % ( str ( itypes ) , ) #revert appearance if not self . useDefaultAppearance and hasattr ( item , 'appearance' ) : item . appearance ( ) . set ( self . oldAppearance )