from ..robotsim import *
from . import simlog
import weakref
from typing import Union,List,Sequence,Callable,Any
from klampt.control.blocks.robotcontroller import RobotControllerBlock
[docs]class SensorEmulator:
"""A generic sensor emulator. Translates from the physics simulation ->
inputs to a Python controller.
The Python controller is assumed to have the structure of
:class:`klampt.control.controller.ControllerBase`, where it is given as
input a dictionary of named items reflecting the most up-to-date readings
on each control time-step.
"""
def __init__(self):
pass
[docs] def update(self) -> dict:
"""Returns a dictionary mapping named sensors to their outputs."""
return {}
[docs] def drawGL(self) -> None:
"""Optional: for debugging"""
return
[docs]class DefaultSensorEmulator(SensorEmulator):
"""A sensor emulator that by default provides the robot's commanded position, velocity,
and other sensors defined in the robot or world XML file.
"""
def __init__(self,sim,controller):
self.sim = sim
self.controller = controller
[docs] def update(self) -> dict:
measurements = {}
mode = self.controller.getControlType()
if mode == "PID":
measurements['qcmd'] = self.controller.getCommandedConfig()
measurements['dqcmd'] = self.controller.getCommandedVelocity()
k = 0
while True:
s = self.controller.sensor(k)
if s.type()=='':
break;
measurements[s.name()] = s.getMeasurements()
k+=1
return measurements
[docs] def drawGL(self) -> None:
if self.controller.getControlType() == "PID":
q = self.controller.getCommandedConfig()
r = self.controller.model()
r.setConfig(q)
colors = []
for j in range(r.numLinks()):
colors.append(r.link(j).appearance().getColor())
r.link(j).appearance().setColor(0,1,0,0.5)
r.drawGL()
for j in range(r.numLinks()):
r.link(j).appearance().setColor(*colors[j])
[docs]class ActuatorEmulator:
"""A generic actuator emulator. Translates outputs from the Python
controller -> the physics simulation. A variety of non-traditional
actuators can be simulated here.
The Python controller is assumed to have the structure of
:class:`~klampt.control.blocks.robotcontroller.RobotControllerBlock', which
outputs a dictionary of commands every control time step. The emulator
will read these with the process() method, and perhaps interact with the
simulator on substep().
"""
def __init__(self):
pass
[docs] def process(self, commands : dict, dt : float) -> None:
"""Processes the dictionary of commands, which are outputted by the controller.
This may involve applying commands to the low-level motor emulator,
or applying forces to the simulator.
Args:
commands (dict): a dictionary of commands produced by the controller
dt (float): the control time step (not the underlying simulation time step)
To play nicely with other actuators in a nested steup, once a command is processed, the class should
remove it from the commands dictionary.
"""
pass
[docs] def substep(self, dt : float) -> None:
"""This is called every simulation substep, which occurs at a higher rate than
process() is called. dt is the simulation substep.
"""
pass
[docs] def drawGL(self) -> None:
"""Optional: for debugging"""
return
[docs]class DefaultActuatorEmulator(ActuatorEmulator):
"""This default emulator can take the commands
- torquecmd: torque comand
- qcmd: position command
- dqcmd: velocity command
- tcmd: time for a dqcmd
And will also pass any remaining commands to the low-level C controller.
"""
def __init__(self,sim,controller):
self.sim = sim
self.controller = controller
[docs] def process(self,commands,dt):
"""Commands: a dictionary of values outputted from the controller module,
or None if no command was issued. """
if commands == None: return
c = self.controller
defaultVals = set(['t','dt','torquecmd','qcmd','dqcmd','tcmd'])
if 'qcmd' in commands:
dqcmd = commands['dqcmd'] if 'dqcmd' in commands else [0.0]*len(commands['qcmd'])
if 'torquecmd' in commands:
c.setPIDCommand(commands['qcmd'],dqcmd,commands['torquecmd'])
else:
c.setPIDCommand(commands['qcmd'],dqcmd)
elif 'dqcmd' in commands:
assert 'tcmd' in commands
c.setVelocity(commands['dqcmd'],commands['tcmd'])
elif 'torquecmd' in commands:
c.setTorque(commands['torquecmd'])
for (k,v) in commands.items():
if k not in defaultVals:
if c.model().sensor(k).type() != '':
continue
if c.sendCommand(k,str(v)):
print("Sent command",k,v,"to low level controller")
return
[docs]class SimpleSimulator (Simulator):
"""A convenience class that enables easy logging, definition of simulation
hooks, emulators of sensors / actuators, and definition of robot
controllers.
Note that for greatest compatibility you should NOT manually apply forces
to the simulation except for inside of hooks and emulators. This is
because several simulation sub-steps will be taken, and you will have
no control over the forces applied except for the first time step.
Args:
world (WorldModel): the world that should be simulated.
"""
def __init__(self, world : WorldModel):
Simulator.__init__(self,world)
#these are functions automatically called at each time step
self.robotControllers = [None]*world.numRobots()
self.sensorEmulators = [[DefaultSensorEmulator(weakref.proxy(self),self.controller(i))] for i in range(world.numRobots())]
self.actuatorEmulators = [[DefaultActuatorEmulator(weakref.proxy(self),self.controller(i))] for i in range(world.numRobots())]
self.hooks = [] # type: List[Callable]
self.hook_args = [] # type: List[Any]
#the rate of applying simulation substeps. Hooks and actuator emulators are
#called at this rate. Note: this should be set at least as large as the simulation time step
self.substep_dt = 0.001
self.worst_status = Simulator.STATUS_NORMAL
#turn this on to save log to disk
self.logging = False
self.logger = None
self.log_state_fn="simulation_state.csv"
self.log_contact_fn="simulation_contact.csv"
#save state so controllers don't rely on world state
self.robotStates = []
self.objectStates = []
[docs] def getStatus(self):
return self.worst_status
[docs] def getStatusString(self, status = -1):
if status > 0:
return Simulator.getStatusString(self, status)
return Simulator.getStatusString(self, self.getStatus())
[docs] def beginLogging(self):
self.logging = True
self.logger = simlog.SimLogger(weakref.proxy(self),self.log_state_fn,self.log_contact_fn)
[docs] def endLogging(self):
self.logging = False
self.logger = None
[docs] def pauseLogging(self,paused=True):
self.logging=not paused
[docs] def toggleLogging(self):
if self.logging:
self.pauseLogging()
else:
if self.logger==None:
self.beginLogging()
else:
self.pauseLogging(False)
[docs] def setController(self, robot : Union[int,str,RobotModel],
function : Union[Callable,RobotControllerBlock]) -> None:
"""Sets a robot's controller function.
Args:
robot: either an index, string, or RobotModel.
function: either 1) a one-argument function that takes the
robot's SimRobotController instance, or 2) an instance of a
:class:`klampt.control.controller.ControllerBlock` class,
which must conform to the
:class:`klampt.control.blocks.robotcontroller.RobotControllerBlock`
convention.
"""
if isinstance(robot,int):
index = robot
elif isinstance(robot,str):
index = self.world.robot(robot).index
elif isinstance(robot,RobotModel):
index = robot.index
else:
raise ValueError("Invalid robot specified")
if not callable(function):
assert hasattr(function,'advance'),"setController takes either a 1-argument function or a ControllerBase instance"
self.robotControllers += [None]*(self.world.numRobots()-len(self.robotControllers))
self.robotControllers[index] = function
[docs] def addEmulator(self, robot : Union[int,str,RobotModel],
e : Union[SensorEmulator,ActuatorEmulator]) -> None:
"""Adds an emulator to the given robot. e must be of SensorEmulator or
ActuatorEmulator type.
"""
if isinstance(robot,int):
index = robot
elif isinstance(robot,str):
index = self.world.robot(robot).index
elif isinstance(robot,RobotModel):
index = robot.index
else:
raise ValueError("Invalid robot specified")
if isinstance(e,SensorEmulator):
self.sensorEmulators[index].append(e)
elif isinstance(e,ActuatorEmulator):
self.actuatorEmulators[index] = [e] + self.actuatorEmulators[index]
else:
raise ValueError("Invalid emulator type")
[docs] def addHook(self, objects, function : Callable) -> None:
"""For the world object(s), applies a hook that gets called every
simulation loop.
Args:
objects: may be an str identifier, a WorldModel entity, or a SimBody.
can also be a list of such objects.
- str: 'time', or the name of any items in the world. If 'time',
the simulation time is passed to function. Otherwise, the
SimBody object named by this str is passed to function.
- RobotModelLink, RigidObjectModel, TerrainModel: the corresponding
SimBody object is passed to function.
- RobotModel: the corresponding SimRobotController object is passed
to function.
- Otherwise, object[i] is passed directly to function.
function (function): a function f(o1,o2,...,on) that is called every
substep with the given object(s) as arguments.
"""
if not hasattr(objects,'__iter__'):
objects = [objects]
args = []
for o in objects:
if isinstance(o,(RobotModelLink,RigidObjectModel,TerrainModel)):
args.append(self.body(o))
elif isinstance(o,RobotModel):
args.append(self.controller(o))
elif isinstance(o,str):
if o == 'time':
args.append(o)
elif self.world.robot(o).world >= 0:
args.append(self.world.robot(o))
elif self.world.terrain(o).world >= 0:
args.append(self.world.terrain(o))
elif self.world.rigidObject(o).world >= 0:
args.append(self.world.rigidObject(o))
else:
raise ValueError("String value "+o+" is unknown")
else:
args.append(o)
self.hooks.append(function)
self.hook_args.append(args)
[docs] def drawGL(self) -> None:
self.updateWorld()
self.world.drawGL()
self.drawEmulatorsGL()
self.drawControllersGL()
[docs] def drawEmulatorsGL(self) -> None:
#draw emulators
for elist in self.sensorEmulators:
for e in elist:
e.drawGL()
for elist in self.actuatorEmulators:
for e in elist:
e.drawGL()
[docs] def drawControllersGL(self) -> None:
#draw controllers
for i in range(self.world.numRobots()):
if self.robotControllers[i] is None:
continue
if not hasattr(self.robotControllers[i],'drawGL'):
continue
self.robotControllers[i].drawGL()
[docs] def simulate(self, dt : float) -> None:
"""Runs the simulation. Note that this should be called at the
rate of the controller. Simulation hooks and emulator substeps
will be called at the rate of substep_dt.
Args:
dt (float): control timestep
"""
#Handle logging
if self.logger: self.logger.saveStep()
self.worst_status = Simulator.STATUS_NORMAL
#Advance controller, emulators
#restore state from previous call -- this is done so that simulation data doesn't leak into controllers
for i,(q,dq) in enumerate(self.robotStates):
self.world.robot(i).setConfig(q)
self.world.robot(i).setVelocity(dq)
for i,T in enumerate(self.objectStates):
self.world.rigidObject(i).setTransform(*T)
#advance controller
self.controlStep(dt)
#save post-controller state
self.robotStates = [(self.world.robot(i).getConfig(),self.world.robot(i).getVelocity()) for i in range(self.world.numRobots())]
self.objectStates = [self.world.rigidObject(i).getTransform() for i in range(self.world.numRigidObjects())]
#advance hooks and the physics simulation at the high rate
assert self.substep_dt > 0
t = 0
while True:
substep = min(self.substep_dt,dt-t)
for i in range(self.world.numRobots()):
for e in self.actuatorEmulators[i]:
e.substep(substep)
for (hook,args) in zip(self.hooks,self.hook_args):
resolvedArgs = []
for a in args:
if isinstance(a,str):
if a=='time':
resolvedArgs.append(self.sim.getTime())
elif a=='dt':
resolvedArgs.append(substep)
else:
raise ValueError("Invalid unresolved argument",a)
else:
resolvedArgs.append(a)
try:
hook(*resolvedArgs)
except Exception as e:
import traceback
print("Hook encountered error with arguments",resolvedArgs)
traceback.print_exc()
raise
#Finally advance the physics simulation
Simulator.simulate(self,substep)
s = Simulator.getStatus(self)
if s > self.worst_status:
self.worst_status = s
t += self.substep_dt
if t >= dt:
break
#done
return
[docs] def controlStep(self, dt :float) -> None:
for i in range(self.world.numRobots()):
c = self.robotControllers[i]
if hasattr(c,'advance'): #it's a block
#build measurement dict
measurements = {'t':self.getTime(),'dt':dt}
for e in self.sensorEmulators[i]:
measurements.update(e.update())
"""
#debug: print measurements
for (k,v) in measurements.iteritems():
print(k,":",)
if hasattr(v,'__iter__'):
print(' '.join("%.2f"%(vi,) for vi in v))
else:
print(v)
"""
if c:
#assume it's a RobotControllerBlock instance
#compute controller output, advance controller
output = c.advance(**measurements)
else:
output = None
#process output => sim using actuator emulators
for e in self.actuatorEmulators[i]:
e.process(output,dt)
elif callable(c): #it's a callable(SimRobotController)
c(self.controller(i))