def __init__(self,world,name="My GL Widget Program"):
     GLRealtimeProgram.__init__(self,name)
     self.world = world
     self.world.robot(0).setConfig(motion.robot.getKlamptCommandedPosition())
     self.widgetMaster = WidgetSet()
     self.widgetButton = 2  #right-clicks
     self.draggingWidget = False
    def __init__(self, simworld=None, planworld=None, controller=None, sim=None, helper=None):

        if simworld == None and planworld == None:
            # Dummy GLViewer
            print "simworld and planworld are None"
            return

        GLRealtimeProgram.__init__(self, "My GL program")

        self.simworld = simworld
        self.planworld = planworld
        self.sim = sim

        self.widgetMaster = WidgetSet()
        self.widgetButton = 2  # right-clicks
        self.draggingWidget = False

        self.poseWidget = PointPoser()
        self.widgetMaster.add(self.poseWidget)
        self.robotWidget = RobotPoser(self.simworld.robot(0))
        self.widgetMaster.add(self.robotWidget)

        self.simulate = True

        if self.sim == None:
            self.simulate = False
        self.controller = controller
        self.command_queue = Queue()

        if helper != None:
            print "helper is not none"
            helper.run()

        self.points = None

        self.extraControllers = {}
class GLWidgetProgram(GLRealtimeProgram):
    """A base class for using widgets.  Right-clicks are passed onto widgets.
    
    Subclasses should call self.widgetMaster.add() upon initializiation to
    add widgets to the program.
    """
    def __init__(self,world,name="My GL Widget Program"):
        GLRealtimeProgram.__init__(self,name)
        self.world = world
        self.world.robot(0).setConfig(motion.robot.getKlamptCommandedPosition())
        self.widgetMaster = WidgetSet()
        self.widgetButton = 2  #right-clicks
        self.draggingWidget = False

    def display(self):
        #Put your display handler here
        #the next few lines draw everything but the robot
        for i in xrange(self.world.numTerrains()):
            self.world.terrain(i).drawGL()
        for i in xrange(self.world.numRigidObjects()):
            self.world.rigidObject(i).drawGL()
        for i in xrange(1,self.world.numRobots()):
            self.world.robot(i).drawGL()
        #this line will draw the robot
        self.widgetMaster.drawGL(self.viewport())

    def idle(self):
        self.widgetMaster.idle()
        if self.widgetMaster.wantsRedraw():
            self.refresh()

    def mousefunc(self,button,state,x,y):
        #print "mouse",button,state,x,y
        if button==self.widgetButton:
            if state==0:
                if self.widgetMaster.beginDrag(x,self.height-y,self.viewport()):
                    self.draggingWidget = True
            else:
                if self.draggingWidget:
                    self.widgetMaster.endDrag()
                    self.draggingWidget = False
            if self.widgetMaster.wantsRedraw():
                self.refresh()
            self.lastx,self.lasty = x,y
            return
        GLRealtimeProgram.mousefunc(self,button,state,x,y)

    def motionfunc(self,x,y):
        if self.draggingWidget:
            self.widgetMaster.drag(x-self.lastx,self.lasty-y,self.viewport())
            if self.widgetMaster.wantsRedraw():
                self.refresh()
            self.lastx,self.lasty = x,y
        else:
            res = self.widgetMaster.hover(x,self.height-y,self.viewport())
            if self.widgetMaster.wantsRedraw():
                self.refresh()
            GLRealtimeProgram.motionfunc(self,x,y)

    def specialfunc(self,c,x,y):
        #Put your keyboard special character handler here
        print c,"pressed"
        self.widgetMaster.keypress(c)
        self.refresh()

    def keyboardfunc(self,c,x,y):
        #Put your keyboard handler here
        #the current example toggles simulation / movie mode
        print c,"pressed"
        self.widgetMaster.keypress(c)
        self.refresh()
class CustomGLViewer(GLRealtimeProgram):
    def __init__(self, simworld=None, planworld=None, controller=None, sim=None, helper=None):

        if simworld == None and planworld == None:
            # Dummy GLViewer
            print "simworld and planworld are None"
            return

        GLRealtimeProgram.__init__(self, "My GL program")

        self.simworld = simworld
        self.planworld = planworld
        self.sim = sim

        self.widgetMaster = WidgetSet()
        self.widgetButton = 2  # right-clicks
        self.draggingWidget = False

        self.poseWidget = PointPoser()
        self.widgetMaster.add(self.poseWidget)
        self.robotWidget = RobotPoser(self.simworld.robot(0))
        self.widgetMaster.add(self.robotWidget)

        self.simulate = True

        if self.sim == None:
            self.simulate = False
        self.controller = controller
        self.command_queue = Queue()

        if helper != None:
            print "helper is not none"
            helper.run()

        self.points = None

        self.extraControllers = {}

    def idle(self):

        if self.extraControllers != {}:
            remove_indices = []
            for controller in self.extraControllers:
                self.dt = 0.01
                controller.sim.simulate(self.dt)
                glutPostRedisplay()

                if controller.remainingTime() <= 0:
                    remove_indices.append(controller)

            for controller in remove_indices:
                del self.extraControllers[controller]
                print "controller removed"

        if self.simulate:
            self.dt = 0.01
            self.sim.simulate(self.dt)
            glutPostRedisplay()
        self.widgetMaster.idle()
        if self.widgetMaster.wantsRedraw():
            print "widgetMaster wants redraw"
            self.refresh()

    def printStuff(self):
        print "shelf xform:", knowledge.shelf_xform
        print self.simworld.terrain(0).geometry()
        # print "terrain xform:", self.simworld.terrain(0).geometry().getTransform(), "\n\n"

    def updatePoints(self, points):
        self.points = points

    def addController(self, new_controller, rgb):
        self.extraControllers[new_controller] = rgb

    def glShowPointCloud(self, pc, downsample_rate=5, pt_size=None):
        # print glGetFloatv(GL_CURRENT_COLOR)
        pc = np.array(pc)
        # print 'Rendering %d points'%pc.shape[0]
        try:
            d = pc.shape[1]
            assert d == 3 or d == 4, "Unrecognized point cloud shape: " + str(pc.shape)
            pc = pc[::downsample_rate, :]
            pc = pc.tolist()
            glDisable(GL_LIGHTING)
            glColor3f(0, 1, 0)
            if pt_size is not None:
                glPointSize(pt_size)
            glBegin(GL_POINTS)
            for p in pc:
                if d == 4:
                    r, g, b = pcl_float_to_rgb(p[3])
                    r /= 255.0
                    g /= 255.0
                    b /= 255.0
                    glColor3f(r, g, b)
                glVertex3f(p[0], p[1], p[2])
            glEnd()
            glEnable(GL_LIGHTING)
            glColor3f(1, 0, 0)
        except:
            # print 'Insufficient data'
            pass

    def updateBox(self, index, vertices):
        BOX_COORDS[index] = vertices

    def display(self):

        global BOX_COORDS

        glEnable(GL_BLEND)

        glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
        glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, [1, 0, 0, 0.5])
        # only 1 robot in this case, but still use for-loop for generality
        for i in xrange(self.simworld.numRobots()):
            r = self.controller.robotModel
            q = self.controller.getCommandedConfig()
            if self.widgetMaster.wantsRedraw():
                # restart sim - start it with the new robot config
                # if simulating
                if self.sim != None:

                    # put more checks on mouse function here
                    # might accidentally reset if you mouse over
                    q = self.robotWidget.get()
                    # print 'self.sim is not None'
                    self.controller.robotModel.setConfig(q)
                    self.simworld.robot(0).setConfig(q)
                    self.controller.setLinear(q, 0.01)
            else:
                self.robotWidget.set(q)

            # q = self.controller.getSensedConfig()
            r.setConfig(q)
            r.drawGL(False)
        glDisable(GL_BLEND)

        for controller in self.extraControllers:
            glEnable(GL_BLEND)
            glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
            glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, self.extraControllers[controller])
            r = controller.robotModel
            q = controller.getCommandedConfig()
            r.setConfig(q)
            r.drawGL(False)
            glDisable(GL_BLEND)

        for i in xrange(self.simworld.numTerrains()):
            self.simworld.terrain(i).drawGL()

        if self.points != None:
            self.glShowPointCloud(self.points)

        for i in BOX_COORDS:
            self.draw_wire_box(*BOX_COORDS[i])

        self.widgetMaster.drawGL(self.viewport())

    def draw_wire_box(self, bmin, bmax):
        """Helper: draws a wireframe box"""
        glBegin(GL_LINE_LOOP)
        glVertex3f(bmin[0], bmin[1], bmin[2])
        glVertex3f(bmin[0], bmin[1], bmax[2])
        glVertex3f(bmin[0], bmax[1], bmax[2])
        glVertex3f(bmin[0], bmax[1], bmin[2])
        glEnd()
        glBegin(GL_LINE_LOOP)
        glVertex3f(bmax[0], bmin[1], bmin[2])
        glVertex3f(bmax[0], bmin[1], bmax[2])
        glVertex3f(bmax[0], bmax[1], bmax[2])
        glVertex3f(bmax[0], bmax[1], bmin[2])
        glEnd()
        glBegin(GL_LINES)
        glVertex3f(bmin[0], bmin[1], bmin[2])
        glVertex3f(bmax[0], bmin[1], bmin[2])
        glVertex3f(bmin[0], bmin[1], bmax[2])
        glVertex3f(bmax[0], bmin[1], bmax[2])
        glVertex3f(bmin[0], bmax[1], bmax[2])
        glVertex3f(bmax[0], bmax[1], bmax[2])
        glVertex3f(bmin[0], bmax[1], bmin[2])
        glVertex3f(bmax[0], bmax[1], bmin[2])
        glEnd()

    def keyboardfunc(self, c, x, y):
        # print c,"pressed"
        # print 'int value =', ord(c)

        self.widgetMaster.keypress(c)
        self.refresh()
        if c == "`":
            self.simulate = not self.simulate
            print "Simulating:", self.simulate
        else:
            self.command_queue.put(c)
            # print int(c)
            if c == chr(27):
                # c == esc
                # self.picking_thread.join()
                exit(0)
        glutPostRedisplay()

    def mousefunc(self, button, state, x, y):
        # print "mouse",button,state,x,y

        if button == self.widgetButton:
            if state == 0:
                # print 'state was zero'
                if self.widgetMaster.beginDrag(x, self.height - y, self.viewport()):
                    # print 'beginning drag'
                    self.draggingWidget = True
            else:
                # print 'state of button not zero'
                if self.draggingWidget:
                    # print 'dragging widget true - ending drag'
                    self.widgetMaster.endDrag()
                    self.draggingWidget = False
            if self.widgetMaster.wantsRedraw():
                self.refresh()
            self.lastx, self.lasty = x, y
            return
        GLRealtimeProgram.mousefunc(self, button, state, x, y)

    def motionfunc(self, x, y, dx, dy):
        if self.draggingWidget:
            self.widgetMaster.drag(dx, -dy, self.viewport())
            if self.widgetMaster.wantsRedraw():
                self.refresh()
        else:
            res = self.widgetMaster.hover(x, self.height - y, self.viewport())
            if self.widgetMaster.wantsRedraw():
                self.refresh()
            GLRealtimeProgram.motionfunc(self, x, y, dx, dy)

    def specialfunc(self, c, x, y):
        # Put your keyboard special character handler here
        # print c,"pressed"
        self.widgetMaster.keypress(c)
        self.refresh()
Beispiel #5
0
 def __init__(self,world,name="My GL Widget Program"):
     GLRealtimeProgram.__init__(self,name)
     self.world = world
     self.widgetMaster = WidgetSet()
     self.widgetButton = 2  #right-clicks
     self.draggingWidget = False
Beispiel #6
0
class GLWidgetProgram(GLRealtimeProgram):
    """A base class for using widgets.  Right-clicks are passed onto widgets.
    
    Subclasses should call self.widgetMaster.add() upon initializiation to
    add widgets to the program.
    """
    def __init__(self, world, name="My GL Widget Program"):
        GLRealtimeProgram.__init__(self, name)
        self.world = world
        self.widgetMaster = WidgetSet()
        self.widgetButton = 2  #right-clicks
        self.draggingWidget = False

    def display(self):
        #Put your display handler here
        #the next few lines draw everything but the robot
        for i in xrange(self.world.numTerrains()):
            self.world.terrain(i).drawGL()
        for i in xrange(self.world.numRigidObjects()):
            self.world.rigidObject(i).drawGL()
        for i in xrange(1, self.world.numRobots()):
            self.world.robot(i).drawGL()
        #this line will draw the robot
        self.widgetMaster.drawGL(self.viewport())

    def idle(self):
        self.widgetMaster.idle()
        if self.widgetMaster.wantsRedraw():
            self.refresh()

    def mousefunc(self, button, state, x, y):
        print "mouse", button, state, x, y
        if button == self.widgetButton:
            if state == 0:
                if self.widgetMaster.beginDrag(x, self.height - y,
                                               self.viewport()):
                    self.draggingWidget = True
            else:
                if self.draggingWidget:
                    self.widgetMaster.endDrag()
                    self.draggingWidget = False
            if self.widgetMaster.wantsRedraw():
                self.refresh()
            self.lastx, self.lasty = x, y
            return
        GLRealtimeProgram.mousefunc(self, button, state, x, y)

    def motionfunc(self, x, y, dx, dy):
        if self.draggingWidget:
            self.widgetMaster.drag(dx, -dy, self.viewport())
            if self.widgetMaster.wantsRedraw():
                self.refresh()
        else:
            res = self.widgetMaster.hover(x, self.height - y, self.viewport())
            if self.widgetMaster.wantsRedraw():
                self.refresh()
            GLRealtimeProgram.motionfunc(self, x, y, dx, dy)

    def specialfunc(self, c, x, y):
        #Put your keyboard special character handler here
        print c, "pressed"
        self.widgetMaster.keypress(c)
        self.refresh()

    def keyboardfunc(self, c, x, y):
        #Put your keyboard handler here
        #the current example toggles simulation / movie mode
        print c, "pressed"
        self.widgetMaster.keypress(c)
        self.refresh()
Beispiel #7
0
 def __init__(self, world, name="My GL Widget Program"):
     GLRealtimeProgram.__init__(self, name)
     self.world = world
     self.widgetMaster = WidgetSet()
     self.widgetButton = 2  #right-clicks
     self.draggingWidget = False