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