"""Defines utilities for connecting simulation controllers to the Robot
Interface Layer, and connect ControllerBlocks to RobotInterfaceBase receivers.
"""
from .controller import ControllerBlock
from .robotinterface import RobotInterfaceBase
from .robotinterfaceutils import RobotInterfaceCompleter
[docs]class SimRobotControllerToInterface(object):
"""A class that provides an API like :class:`SimRobotController` so that
simulation control code can be ported easily to a :class:`RobotInterfaceBase`
Robot Interface Layer API.
Missing:
- setRate(dt): can't change a real robot's control rate.
- getSetting()/setSetting(): no interface to configure robot.
- setMilestone(q,dq): can't set a terminal velocity.
- add*(): can't queue up commands.
- getPIDGains(): can't configure PID gains.
Arguments:
robotInterface (RobotInterfaceBase): the robot interface to use
"""
def __init__(self,robotInterface):
if not isinstance(robotInterface,RobotInterfaceCompleter):
robotInterface = RobotInterfaceCompleter(robotInterface)
self.robotInterface = robotInterface
[docs] def initialize(self):
self.robotInterface.initialize()
[docs] def startStep(self):
self.robotInterface.startStep()
[docs] def endStep(self):
self.robotInterface.endStep()
[docs] def model(self):
return self.robotInterface.klamptModel()
[docs] def setRate(self,dt):
raise NotImplementedError("setRate() may not be called")
[docs] def getRate(self):
return 1.0/self.robotInterface.controlRate()
[docs] def getCommandedConfig(self):
return self.robotInterface.configToKlampt(self.robotInterface.commandedPosition())
[docs] def getCommandedVelocity(self):
return self.robotInterface.velocityToKlampt(self.robotInterface.commandedVelocity())
[docs] def getCommandedTorque(self):
return self.robotInterface.commandedTorque()
[docs] def getSensedConfig(self):
return self.robotInterface.configToKlampt(self.robotInterface.sensedPosition())
[docs] def getSensedVelocity(self):
return self.robotInterface.velocityToKlampt(self.robotInterface.sensedVelocity())
[docs] def getSensedTorque(self):
return self.robotInterface.sensedTorque()
[docs] def sensor(self,index_or_name):
if isinstance(index_or_name,int):
sensorNames = self.robotInterface.sensors()
if index_or_name < 0 or index_or_name >= len(sensorNames):
#"null" sensor
return _SimRobotSensorFromInterface(self.robotInterface,None)
name = sensorNames[index_or_name]
else:
name = index_or_name
enabled = self.robotInterface.enabledSensors()
if name not in enabled:
if not self.robotInterface.hasSensor(name):
#"null" sensor
return _SimRobotSensorFromInterface(self.robotInterface,None)
self.robotInterface.enableSensor(name)
return _SimRobotSensorFromInterface(self.robotInterface,name)
[docs] def commands(self):
return ['estop','softStop','reset']
[docs] def sendCommand(self,name,args):
getattr(self,name)()
[docs] def getSetting(self,name):
raise ValueError("Can't change settings")
[docs] def setSetting(self,name,val):
raise ValueError("Can't change settings")
[docs] def setMilestone(self,q,dq=None):
if dq is not None:
raise NotImplementedError("setMilestone(q[,dq]) may not be called with dq")
self.robotInterface.moveToPosition(self.robotInterface.configFromKlampt(q))
[docs] def addMilestone(self,q,dq=None):
raise NotImplementedError("addMilestone() may not be called")
[docs] def addMilestoneLinear(self,q):
raise NotImplementedError("addMilestoneLinear() may not be called")
[docs] def setLinear(self,q,dt):
self.robotInterface.setPiecewiseLinear([dt],[self.robotInterface.configFromKlampt(q)])
[docs] def setCubic(self,q,v,dt):
self.robotInterface.setPiecewiseCubic([dt],[self.robotInterface.configFromKlampt(q)],[self.robotInterface.velocityFromKlampt(v)])
[docs] def addLinear(self,q,dt):
raise NotImplementedError("addLinear() may not be called")
[docs] def addCubic(self,q,dt):
raise NotImplementedError("addCubic() may not be called")
[docs] def remainingTime(self):
return self.robotInterface.destinationTime()
[docs] def setVelocity(self,dq,dt):
self.robotInterface.setVelocity(self.robotInterface.velocityFromKlampt(dq),dt)
[docs] def setTorque(self,t):
self.robotInterface.setTorque(t)
[docs] def setPIDCommand(self,qdes,dqdes,tfeedforward=None):
self.robotInterface.setPIDCommand(self.robotInterface.configFromKlampt(qdes),self.robotInterface.velocityFromKlampt(dqdes),tfeedforward)
[docs] def setManualMode(self,enabled):
pass
[docs] def getControlType(self):
return 'unknown'
[docs] def setPIDGains(self,kP,kI,kD):
self.robotInterface.setPIDGains(kP,kI,kD)
[docs] def getPIDGains(self):
return None,None,None
class _SimRobotSensorFromInterface(object):
def __init__(self,iface,name):
self.iface = iface
self._name = name
def name(self):
if self._name is None: return ''
return self._name
def type(self):
if self._name is None: return ''
return 'UnknownSensor'
def robot(self):
return self.iface.klamptModel()
def measurementNames(self):
if self._name is None: return []
return self.robot().sensor(self._name).measurementNames()
def getMeasurements(self):
if self._name is None: return []
return self.iface.sensorMeasurements(self._name)
def getSetting(self,name):
return self.robot().sensor(self._name).getSetting(name)
def setSetting(self,name,val):
raise NotImplementedError("setSetting may not be called")
def drawGL(self,measurements=None):
if measurements is not None:
return self.robot().sensor(self._name).drawGL(measurements)
return self.robot().sensor(self._name).drawGL()
[docs]class RobotControllerToInterface(object):
"""A class that connects the I/O of a standard :class:`ControllerBlock`
robot controller with a :class:`RobotInterfaceBase` Robot Interface Layer
API.
Arguments:
controller (ControllerBlock): the controller software, should conform
to the RobotControllerBase convention.
robotInterface (RobotInterfaceBase): the robot interface
controllerRateRatio (float, optional): if not 1, scales how many times
the controller is run for each main loop of the robotInterface
"""
def __init__(self,controller,robotInterface,controllerRateRatio=1):
if not isinstance(robotInterface,RobotInterfaceCompleter):
robotInterface = RobotInterfaceCompleter(robotInterface)
self.robotInterface = robotInterface
self.controller = controller
self.controllerRateRatio = controllerRateRatio
self.lastInputs = None
self.lastOutputs = None
[docs] def advance(self):
if self.controllerRateRatio != 1:
raise NotImplementedError("Can't do controller / robotInterface time step ratio != 1")
self.robotInterface.startStep()
inputs = dict()
inputs['t'] = self.robotInterface.clock()
inputs['dt'] = 1.0/self.robotInterface.controlRate()
inputs['q'] = self.robotInterface.sensedPosition()
inputs['dq'] = self.robotInterface.sensedVelocity()
try:
inputs['torque'] = self.robotInterface.sensedTorque()
except NotImplementedError:
pass
for s in self.robotInterface.enabledSensors():
inputs[s] = self.robotInterface.sensorMeasurements(s)
res = self.controller.advance(**inputs)
if 'qcmd' in res:
dqcmd = res['dqcmd'] if 'dqcmd' in res else [0.0]*len(res['qcmd'])
if 'torquecmd' in res:
self.robotInterface.setPID(res['qcmd'],dqcmd,res['torquecmd'])
else:
self.robotInterface.setPID(res['qcmd'],dqcmd)
elif 'dqcmd' in res:
assert 'tcmd' in res
self.robotInterface.setVelocity(res['dqcmd'],res['tcmd'])
elif 'torquecmd' in res:
self.robotInterface.setTorque(res['torquecmd'])
self.robotInterface.endStep()
self.lastInputs = inputs
self.lastOutputs = res
[docs]class RobotInterfacetoVis(object):
"""A class that manages connections between the sensor readings of a
:class:`RobotInterfaceBase` Robot Interface Layer API to the Klamp't vis
module.
This assumes that ``vis`` has been set up with an appropriate world.
"""
def __init__(self,robotInterface,visRobotIndex=0):
self.interface = robotInterface
self.visRobotIndex = visRobotIndex
self.text_x = 10
self.text_y = 10
self.tag = '_'+str(visRobotIndex)+'_'
[docs] def update(self):
from klampt import vis
t = 0
try:
t = self.interface.clock()
vis.addText(self.tag+"clock",'%.3f'%(t,),(self.text_x,self.text_y))
except NotImplementedError:
vis.addText(self.tag+"clock",str(self.interface)+" provides no clock",(self.text_x,self.text_y))
try:
stat = self.interface.status()
if stat != 'ok':
vis.addText(self.tag+"status",'Status: '+stat,(self.text_x,self.text_y+15),color=(1,0,0))
else:
try:
vis.remove(self.tag+"status")
except Exception:
pass
except NotImplementedError:
vis.addText(self.tag+"status",str(self.interface)+" provides no status",(self.text_x,self.text_y+15))
moving = False
endTime = 0
try:
moving = self.interface.isMoving()
if moving:
endTime = self.interface.destinationTime()
if moving:
if endTime > t:
vis.addText(self.tag+"moving","Moving, %.3fs left"%(endTime-t,),(self.text_x,self.text_y+30))
else:
vis.addText(self.tag+"moving","Moving",(self.text_x,self.text_y+30))
else:
try:
vis.remove(self.tag+"moving")
except Exception:
pass
except NotImplementedError:
pass
try:
qsns = self.interface.configToKlampt(self.interface.sensedPosition())
vis.add(self.tag+"q_sns",qsns,robot=self.visRobotIndex,color=(0,1,0,0.5))
except NotImplementedError:
qsns = None
try:
qcmd = self.interface.configToKlampt(self.interface.commandedPosition())
if qcmd != qsns:
vis.add(self.tag+"q_cmd",qcmd,robot=self.visRobotIndex,color=(1,1,0,0.5))
except NotImplementedError:
pass
try:
if moving and endTime > t:
qdes = self.interface.configToKlampt(self.interface.destinationPosition())
vis.add(self.tag+"q_dest",qdes,robot=self.visRobotIndex,color=(1,0,0,0.5))
else:
try:
vis.remove(self.tag+"q_dest")
except Exception:
pass
except NotImplementedError:
pass