class GLSimulationProgram(GLRealtimeProgram): """A program that runs a simulation given a world. Attributes: - world: the RobotWorld instance provided on startup. All elements are assumed to be instantiated already. - sim: a Simulator for the given world. - simulate: set this to True to start simulating. - saveScreenshots: set this to True if frames should be saved to disk. - commanded_config_color: an RGBA tuple defining the color of the commanded configuration, or None if it should not be drawn. - verbose: set to 1 if you wish to get printouts of the event loop - logging, logger: set to True and the logger if you wish to save a CSV log file to disk. Easier to use beginLogging(), pauseLogging(), and endLogging(). Subclasses should overload self.control_loop() and put whatever control loop you desire inside. Note: in this loop you should interact with self.sim.controller(0), not self.world.robot(0). self.world is simply a model and does not have a direct relation to the simulation. """ def __init__(self,world,name="My GL simulation program"): """Arguments: - world: a RobotWorld instance. """ GLRealtimeProgram.__init__(self,name) self.world = world #Put your initialization code here #the current example creates a collision class, simulator, #simulation flag, and screenshot flags self.collider = robotcollide.WorldCollider(world) self.sim = SimpleSimulator(world) self.simulate = False self.commanded_config_color = [0,1,0,0.5] #turn this on to draw contact points self.drawContacts = False #turn this on to save screenshots self.saveScreenshots = False self.nextScreenshotTime = 0 self.screenshotCount = 0 self.verbose = 0 #turn this on to save log to disk self.logging = False self.logger = None def beginLogging(self,state_fn="simulation_state.csv",contact_fn="simulation_contact.csv"): self.logging = True self.logger = SimLogger(self.sim,state_fn,contact_fn) def endLogging(self): self.logging = False self.logger = None def pauseLogging(self,paused=True): self.logging=not paused def display(self): #Put your display handler here #the current example draws the simulated world in grey and the #commanded configurations in transparent green self.sim.drawGL() """ #draw commanded configurations if self.commanded_config_color != None: for i in xrange(self.world.numRobots()): r = self.world.robot(i) mode = self.sim.controller(i).getControlType() if mode == "PID": q = self.sim.controller(i).getCommandedConfig() #save old appearance oldapps = [r.link(j).appearance().clone() for j in xrange(r.numLinks())] #set new appearance for j in xrange(r.numLinks()): r.link(j).appearance().setColor(*self.commanded_config_color) r.setConfig(q) r.drawGL() #restore old appearance for j in xrange(r.numLinks()): r.link(j).appearance().set(oldapps[j]) glDisable(GL_BLEND) """ #draw contacts, if enabled if self.drawContacts: glDisable(GL_LIGHTING) glDisable(GL_DEPTH_TEST) glEnable(GL_POINT_SMOOTH) glColor3f(1,1,0) glLineWidth(1.0) glPointSize(5.0) forceLen = 0.1 #scale of forces maxid = self.world.numIDs() for i in xrange(maxid): for j in xrange(i+1,maxid): points = self.sim.getContacts(i,j) if len(points) > 0: forces = self.sim.getContactForces(i,j) glBegin(GL_POINTS) for p in points: glVertex3f(*p[0:3]) glEnd() glBegin(GL_LINES) for p,f in zip(points,forces): glVertex3f(*p[0:3]) glVertex3f(*vectorops.madd(p[0:3],f,forceLen)) glEnd() glEnable(GL_DEPTH_TEST) def control_loop(self): #Put your control handler here pass def idle(self): #Put your idle loop handler here #the current example simulates with the current time step self.dt if self.simulate: #Handle screenshots if self.saveScreenshots: #The following line saves movies on simulation time if self.sim.getTime() >= self.nextScreenshotTime: #The following line saves movies on wall clock time #if self.ttotal >= self.nextScreenshotTime: self.save_screen("image%04d.ppm"%(self.screenshotCount,)) self.screenshotCount += 1 self.nextScreenshotTime += 1.0/30.0; #Handle logging if self.logger: self.logger.saveStep() #Advance simulation self.control_loop() self.sim.simulate(self.dt) self.refresh() def mousefunc(self,button,state,x,y): #Put your mouse handler here #the current example prints out the list of objects clicked whenever #you right click if self.verbose: print "mouse",button,state,x,y GLRealtimeProgram.mousefunc(self,button,state,x,y) def motionfunc(self,x,y,dx,dy): GLRealtimeProgram.motionfunc(self,x,y,dx,dy) def specialfunc(self,c,x,y): #Put your keyboard special character handler here if self.verbose: print c,"pressed" pass def keyboardfunc(self,c,x,y): #Put your keyboard handler here #the current example toggles simulation / movie mode if self.verbose: print c,"pressed" if c == 's': self.simulate = not self.simulate print "Simulating:",self.simulate elif c == 'm': self.saveScreenshots = not self.saveScreenshots print "Movie mode:",self.saveScreenshots elif c == 'l': if self.logging: self.pauseLogging() else: if self.logger==None: self.beginLogging() else: self.pauseLogging(False) elif c == 'c': self.drawContacts = not self.drawContacts if self.drawContacts: self.sim.enableContactFeedbackAll() self.refresh() def click_world(self,x,y): """Helper: returns a list of world objects sorted in order of increasing distance.""" #get the viewport ray (s,d) = self.click_ray(x,y) #run the collision tests collided = [] for g in self.collider.geomList: (hit,pt) = g[1].rayCast(s,d) if hit: dist = vectorops.dot(vectorops.sub(pt,s),d) collided.append((dist,g[0])) return [g[1] for g in sorted(collided)]
class GLSimulationProgram(GLRealtimeProgram): """A program that runs a simulation given a world. Attributes: - world: the RobotWorld instance provided on startup. All elements are assumed to be instantiated already. - sim: a Simulator for the given world. - simulate: set this to True to start simulating. - saveScreenshots: set this to True if frames should be saved to disk. - commanded_config_color: an RGBA tuple defining the color of the commanded configuration, or None if it should not be drawn. - verbose: set to 1 if you wish to get printouts of the event loop - logging, logger: set to True and the logger if you wish to save a CSV log file to disk. Easier to use beginLogging(), pauseLogging(), and endLogging(). Subclasses should overload self.control_loop() and put whatever control loop you desire inside. Note: in this loop you should interact with self.sim.controller(0), not self.world.robot(0). self.world is simply a model and does not have a direct relation to the simulation. """ def __init__(self, world, name="My GL simulation program"): """Arguments: - world: a RobotWorld instance. """ GLRealtimeProgram.__init__(self, name) self.world = world # Put your initialization code here # the current example creates a collision class, simulator, # simulation flag, and screenshot flags self.collider = robotcollide.WorldCollider(world) self.sim = SimpleSimulator(world) self.simulate = False self.commanded_config_color = [0, 1, 0, 0.5] # turn this on to draw contact points self.drawContacts = False # turn this on to save screenshots self.saveScreenshots = False self.nextScreenshotTime = 0 self.screenshotCount = 0 self.verbose = 0 # turn this on to save log to disk self.logging = False self.logger = None def beginLogging(self, state_fn="simulation_state.csv", contact_fn="simulation_contact.csv"): self.logging = True self.logger = SimLogger(self.sim, state_fn, contact_fn) def endLogging(self): self.logging = False self.logger = None def pauseLogging(self, paused=True): self.logging = not paused def display(self): # Put your display handler here # the current example draws the simulated world in grey and the # commanded configurations in transparent green self.sim.drawGL() """ #draw commanded configurations if self.commanded_config_color != None: for i in xrange(self.world.numRobots()): r = self.world.robot(i) mode = self.sim.controller(i).getControlType() if mode == "PID": q = self.sim.controller(i).getCommandedConfig() #save old appearance oldapps = [r.link(j).appearance().clone() for j in xrange(r.numLinks())] #set new appearance for j in xrange(r.numLinks()): r.link(j).appearance().setColor(*self.commanded_config_color) r.setConfig(q) r.drawGL() #restore old appearance for j in xrange(r.numLinks()): r.link(j).appearance().set(oldapps[j]) glDisable(GL_BLEND) """ # draw contacts, if enabled if self.drawContacts: glDisable(GL_LIGHTING) glDisable(GL_DEPTH_TEST) glEnable(GL_POINT_SMOOTH) glColor3f(1, 1, 0) glLineWidth(1.0) glPointSize(5.0) forceLen = 0.1 # scale of forces maxid = self.world.numIDs() for i in xrange(maxid): for j in xrange(i + 1, maxid): points = self.sim.getContacts(i, j) if len(points) > 0: forces = self.sim.getContactForces(i, j) glBegin(GL_POINTS) for p in points: glVertex3f(*p[0:3]) glEnd() glBegin(GL_LINES) for p, f in zip(points, forces): glVertex3f(*p[0:3]) glVertex3f(*vectorops.madd(p[0:3], f, forceLen)) glEnd() glEnable(GL_DEPTH_TEST) def control_loop(self): # Put your control handler here pass def idle(self): # Put your idle loop handler here # the current example simulates with the current time step self.dt if self.simulate: # Handle screenshots if self.saveScreenshots: # The following line saves movies on simulation time if self.sim.getTime() >= self.nextScreenshotTime: # The following line saves movies on wall clock time # if self.ttotal >= self.nextScreenshotTime: self.save_screen("image%04d.ppm" % (self.screenshotCount,)) self.screenshotCount += 1 self.nextScreenshotTime += 1.0 / 30.0 # Handle logging if self.logger: self.logger.saveStep() # Advance simulation self.control_loop() self.sim.simulate(self.dt) self.refresh() def mousefunc(self, button, state, x, y): # Put your mouse handler here # the current example prints out the list of objects clicked whenever # you right click if self.verbose: print "mouse", button, state, x, y GLRealtimeProgram.mousefunc(self, button, state, x, y) def motionfunc(self, x, y, dx, dy): GLRealtimeProgram.motionfunc(self, x, y, dx, dy) def specialfunc(self, c, x, y): # Put your keyboard special character handler here if self.verbose: print c, "pressed" pass def keyboardfunc(self, c, x, y): # Put your keyboard handler here # the current example toggles simulation / movie mode if self.verbose: print c, "pressed" if c == "s": self.simulate = not self.simulate print "Simulating:", self.simulate elif c == "m": self.saveScreenshots = not self.saveScreenshots print "Movie mode:", self.saveScreenshots elif c == "l": if self.logging: self.pauseLogging() else: if self.logger == None: self.beginLogging() else: self.pauseLogging(False) elif c == "c": self.drawContacts = not self.drawContacts if self.drawContacts: self.sim.enableContactFeedbackAll() self.refresh() def click_world(self, x, y): """Helper: returns a list of world objects sorted in order of increasing distance.""" # get the viewport ray (s, d) = self.click_ray(x, y) # run the collision tests collided = [] for g in self.collider.geomList: (hit, pt) = g[1].rayCast(s, d) if hit: dist = vectorops.dot(vectorops.sub(pt, s), d) collided.append((dist, g[0])) return [g[1] for g in sorted(collided)]