def setWorldSimState(world,state):
"""Sets the world state to the prior saved state (a dict from
getWorldSimState())
NOTE: this does not perfectly save simulation state! To do that,
you must use the Simulator().getState()/saveState() methods.
"""
for (k,v) in state.iteritems():
map.set_item(world,k,v)
return
def setWorldSimState(world, state):
"""Sets the world state to the prior saved state (a dict from
getWorldSimState())
NOTE: this does not perfectly save simulation state! To do that,
you must use the Simulator().getState()/saveState() methods.
"""
for (k, v) in state.iteritems():
map.set_item(world, k, v)
return
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 items 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.
- 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 list of final states upon termination. This
takes the form of a dictionary mapping named items (specified by
the returnItems argument) to their values.
"""
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)
for k,v in initialCondition.iteritems():
map.set_item(world,k,v)
sim = Simulator(world)
if simInit: simInit(sim)
assert simDt > 0,"Time step must be positive"
res = dict()
if trace:
for k in returnItems:
res[k] = [map.get_item(sim,k)]
print "klampt.batch.doSim(): Running simulation for",duration,"s"
t0 = time.time()
t = 0
while t < duration:
sim.updateWorld()
if trace:
for k in returnItems:
res[k].append(map.get_item(sim,k))
if simTerm and simTerm(sim)==True:
if not trace:
for k in returnItems:
res[k] = map.get_item(sim,k)
#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)
sim.simulate(simDt)
t += simDt
sim.updateWorld()
if not trace:
for k in returnItems:
res[k] = map.get_item(sim,k)
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.
Arguments:
- world: the world
- duration: the maximum duration of simulation, in seconds
- initialCondition: a dictionary mapping named items to values.
Each named items 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.
- 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 list of final states upon termination. This
takes the form of a dictionary mapping named items (specified by
the returnItems argument) to their values.
"""
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)
for k, v in initialCondition.iteritems():
map.set_item(world, k, v)
sim = Simulator(world)
if simInit: simInit(sim)
assert simDt > 0, "Time step must be positive"
res = dict()
if trace:
for k in returnItems:
res[k] = [map.get_item(sim, k)]
print "klampt.batch.doSim(): Running simulation for", duration, "s"
t0 = time.time()
t = 0
while t < duration:
sim.updateWorld()
if trace:
for k in returnItems:
res[k].append(map.get_item(sim, k))
if simTerm and simTerm(sim) == True:
if not trace:
for k in returnItems:
res[k] = map.get_item(sim, k)
#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)
sim.simulate(simDt)
t += simDt
sim.updateWorld()
if not trace:
for k in returnItems:
res[k] = map.get_item(sim, k)
print " Done."
print " Computation time:", time.time() - t0
#restore initial world state
setWorldSimState(world, initCond)
return res
