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 __init__(self, source_streams, noise_streams, rir_streams, config, DEBUG=False): """ :param source_streams: a list of SpeechDataStream objects, containing the clean speech source files names and meta-data such as label, utterance ID, and speaker ID. :param noise_streams: a list of DataStream objects, containing noise file names. :param rir_streams: a list of RIRDataStream objects, containing RIR file names and meta data information. :param config: an object of type DataGeneratorSequenceConfig :param DEBUG: if set to DEBUG mode, will plot the filterbanks and label. """ self._source_streams = source_streams self._source_streams_prior = self._get_streams_prior(source_streams) self._rir_streams = rir_streams self._rir_streams_prior = self._get_streams_prior(rir_streams) self._noise_streams = noise_streams self._noise_streams_prior = self._get_streams_prior(noise_streams) self._data_len = config.n_segment_per_epoch self._single_source_simulator = SimpleSimulator( use_rir=config.use_reverb, use_noise=config.use_noise, snr_range=config.snr_range) self._config = config self._DEBUG = DEBUG self._gen_window()
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
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)]
def doSim(world, duration, initialCondition, returnItems=None, trace=False, simDt=0.01, simInit=None, simStep=None, simTerm=None): """Runs a simulation for a given initial condition of a world. Arguments: - world: the world - duration: the maximum duration of simulation, in seconds - initialCondition: a dictionary mapping named items to values. Each named item is specified by a path as used by the map module, e.g. 'robot[0].config[4]'. See the documentation for map.get_item()/ map.set_item() for details. Special items include 'args' which is a tuple provided to each simInit, simStep, and simTerm call. - returnItems (optional): a list of named items to return in the final state of the simulation. By default returns everything that is variable in the simulator (simulation time, robot and rigid object configuration / velocity, robot commands, robot sensors). - trace (optional, default False): if True, returns the entire trace of the items specified in returnItems rather than just the final state. - simDt (optional, default 0.01): the outer simulation loop (usually corresponds to the control rate). - simInit (optional): a function f(sim) called on the simulator after its initial conditions are set but before simulating. You may configure the simulator with this function. - simStep (optional): a function f(sim) that is called on every outer simulation loop (usually a controller function). - simTerm (optional): a function f(sim) that returns True if the simulation should terminate early. Called on every outer simulation loop. Return value is the final state of each returned item upon termination. This takes the form of a dictionary mapping named items (specified by the returnItems argument) to their values. Additional returned items are: - 'status', which gives the status string of the simulation - 'time', which gives the time of the simulation, in s - 'wall_clock_time', which gives the time elapsed while computing the simulation, in s """ if returnItems == None: #set up default return items returnItems = [] for i in range(world.numRigidObjects()): returnItems.append('rigidObjects[' + str(i) + '].transform') returnItems.append('rigidObjects[' + str(i) + '].velocity') for i in range(world.numRobots()): returnItems.append('time') returnItems.append('controllers[' + str(i) + '].commandedConfig') returnItems.append('controllers[' + str(i) + '].commandedVelocity') returnItems.append('controllers[' + str(i) + '].sensedConfig') returnItems.append('controllers[' + str(i) + '].sensedVelocity') returnItems.append('controllers[' + str(i) + '].sensors') returnItems.append('robots[' + str(i) + '].actualConfig') returnItems.append('robots[' + str(i) + '].actualVelocity') returnItems.append('robots[' + str(i) + '].actualTorques') initCond = getWorldSimState(world) args = () for k, v in initialCondition.iteritems(): if k is not 'args': map.set_item(world, k, v) else: args = v sim = SimpleSimulator(world) if simInit: simInit(sim, *args) assert simDt > 0, "Time step must be positive" res = dict() if trace: for k in returnItems: res[k] = [map.get_item(sim, k)] res['status'] = [sim.getStatusString()] print "klampt.batch.doSim(): Running simulation for", duration, "s" t0 = time.time() t = 0 worst_status = 0 while t < duration: if simTerm and simTerm(sim, *args) == True: if not trace: for k in returnItems: res[k] = map.get_item(sim, k) res['status'] = sim.getStatusString(worst_status) res['time'] = t res['wall_clock_time'] = time.time() - t0 #restore initial world state setWorldSimState(world, initCond) print " Termination condition reached at", t, "s" print " Computation time:", time.time() - t0 return res if simStep: simStep(sim, *args) sim.simulate(simDt) worst_status = max(worst_status, sim.getStatus()) if trace: for k in returnItems: res[k].append(map.get_item(sim, k)) res['status'].append(sim.getStatusString()) res['time'] = t res['wall_clock_time'] = time.time() - t0 t += simDt if not trace: #just get the terminal stats for k in returnItems: res[k] = map.get_item(sim, k) res['status'] = sim.getStatusString(worst_status) res['time'] = t res['wall_clock_time'] = time.time() - t0 print " Done." print " Computation time:", time.time() - t0 #restore initial world state setWorldSimState(world, initCond) return res
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)]
def doSim(world,duration,initialCondition, returnItems=None,trace=False, simDt=0.01,simInit=None,simStep=None,simTerm=None): """Runs a simulation for a given initial condition of a world. Args: world (WorldModel): the world duration (float): the maximum duration of simulation, in seconds initialCondition (dict): a dictionary mapping named items to values. Each named item is specified by a path as used by the access module, e.g. 'robot[0].config[4]'. See the documentation for access.get_item()/ access.set_item() for details. Special items include 'args' which is a tuple provided to each simInit, simStep, and simTerm call. returnItems (list of strs, optional): a list of named items to return in the final state of the simulation. By default returns everything that is variable in the simulator (simulation time, robot and rigid object configuration / velocity, robot commands, robot sensors). trace (bool, optional): if True, returns the entire trace of the items specified in returnItems rather than just the final state. simDt (float, optional, default 0.01): the outer simulation loop (usually corresponds to the control rate). simInit (function, optional): a function f(sim) called on the simulator after its initial conditions are set but before simulating. You may configure the simulator with this function. simStep (function, optional): a function f(sim) that is called on every outer simulation loop (usually a controller function). simTerm (function, optional): a function f(sim) that returns True if the simulation should terminate early. Called on every outer simulation loop. Returns: (dict): the final state of each returned item upon termination. The dictionary maps named items (specified by the returnItems argument) to their values. Additional returned items are: * 'status', which gives the status string of the simulation * 'time', which gives the time of the simulation, in s * 'wall_clock_time', which gives the time elapsed while computing the simulation, in s """ if returnItems == None: #set up default return items returnItems = [] for i in range(world.numRigidObjects()): returnItems.append('rigidObjects['+str(i)+'].transform') returnItems.append('rigidObjects['+str(i)+'].velocity') for i in range(world.numRobots()): returnItems.append('time') returnItems.append('controllers['+str(i)+'].commandedConfig') returnItems.append('controllers['+str(i)+'].commandedVelocity') returnItems.append('controllers['+str(i)+'].sensedConfig') returnItems.append('controllers['+str(i)+'].sensedVelocity') returnItems.append('controllers['+str(i)+'].sensors') returnItems.append('robots['+str(i)+'].actualConfig') returnItems.append('robots['+str(i)+'].actualVelocity') returnItems.append('robots['+str(i)+'].actualTorques') initCond = getWorldSimState(world) args = () for k,v in initialCondition.iteritems(): if k is not 'args': access.set_item(world,k,v) else: args = v sim = SimpleSimulator(world) if simInit: simInit(sim,*args) assert simDt > 0,"Time step must be positive" res = dict() if trace: for k in returnItems: res[k] = [access.get_item(sim,k)] res['status'] = [sim.getStatusString()] print "klampt.batch.doSim(): Running simulation for",duration,"s" t0 = time.time() t = 0 worst_status = 0 while t < duration: if simTerm and simTerm(sim,*args)==True: if not trace: for k in returnItems: res[k] = access.get_item(sim,k) res['status']=sim.getStatusString(worst_status) res['time']=t res['wall_clock_time']=time.time()-t0 #restore initial world state setWorldSimState(world,initCond) print " Termination condition reached at",t,"s" print " Computation time:",time.time()-t0 return res if simStep: simStep(sim,*args) sim.simulate(simDt) worst_status = max(worst_status,sim.getStatus()) if trace: for k in returnItems: res[k].append(access.get_item(sim,k)) res['status'].append(sim.getStatusString()) res['time']=t res['wall_clock_time']=time.time()-t0 t += simDt if not trace: #just get the terminal stats for k in returnItems: res[k] = access.get_item(sim,k) res['status']=sim.getStatusString(worst_status) res['time']=t res['wall_clock_time']=time.time()-t0 print " Done." print " Computation time:",time.time()-t0 #restore initial world state setWorldSimState(world,initCond) return res