def __init__(self, robot, estimator='default'):
self.estimator = estimator
if estimator == 'default':
self.estimator = AdaptiveMotionModel('velocity', 'velocity')
self.xnames = ['q', 'dq']
self.unames = ['dqcmd']
self.qlast = None
self.discount = None
def __init__(self,robot,estimator = 'default'):
self.estimator = estimator
if estimator == 'default':
self.estimator = AdaptiveMotionModel('velocity','velocity')
self.xnames = ['q','dq']
self.unames = ['dqcmd']
self.qlast = None
self.discount = None
class SystemIDEstimator(BaseController):
"""An estimator that runs by updating the adaptive motion model object.
It does not output anything."""
def __init__(self, robot, estimator='default'):
self.estimator = estimator
if estimator == 'default':
self.estimator = AdaptiveMotionModel('velocity', 'velocity')
self.xnames = ['q', 'dq']
self.unames = ['dqcmd']
self.qlast = None
self.discount = None
def getMotionModel(self):
return self.estimator
def signal(self, type, **inputs):
if type == 'reset':
self.xlast = None
#TODO: reset the motion model object?
pass
return
def advance(self, **inputs):
try:
qcmd = inputs['qcmd']
q = inputs['q']
dt = inputs['dt']
except KeyError:
return
if len(q) == 0: return
print len(qcmd), len(q)
u = vectorops.sub(qcmd, q)
u = vectorops.div(u, dt)
print "u estimate:", u
print "u estimate rarm:", u[26:33]
try:
q = inputs[self.xnames[0]]
dq = inputs[self.xnames[1]]
except KeyError as e:
print "Warning, inputs do not contain x key", e
return
"""
try:
u = sum((inputs[uname] for uname in self.unames),[])
except KeyError as e:
print "Warning, inputs do not contain u key",e
#u = [0]*(len(x)/2)
try:
u = vectorops.sub(inputs['qcmd'],inputs['q'])
u = vectorops.div(u,inputs['dt'])
except KeyError as e:
print "Warning, inputs do not contain qcmd key either",e
u = [0]*(len(x)/2)
"""
#self.xlast = x
#return
#do system ID
if self.qlast != None:
self.estimator.add(self.qlast, self.dqlast, u, q, dq)
pass
#update last state
self.qlast = q
self.dqlast = dq
return
class SystemIDEstimator(BaseController):
"""An estimator that runs by updating the adaptive motion model object.
It does not output anything."""
def __init__(self,robot,estimator = 'default'):
self.estimator = estimator
if estimator == 'default':
self.estimator = AdaptiveMotionModel('velocity','velocity')
self.xnames = ['q','dq']
self.unames = ['dqcmd']
self.qlast = None
self.discount = None
def getMotionModel(self):
return self.estimator
def signal(self,type,**inputs):
if type=='reset':
self.xlast = None
#TODO: reset the motion model object?
pass
return
def advance(self,**inputs):
try:
qcmd = inputs['qcmd']
q = inputs['q']
dt = inputs['dt']
except KeyError:
return
if len(q)==0: return
print len(qcmd),len(q)
u = vectorops.sub(qcmd,q)
u = vectorops.div(u,dt)
print "u estimate:",u
print "u estimate rarm:",u[26:33]
try:
q = inputs[self.xnames[0]]
dq = inputs[self.xnames[1]]
except KeyError as e:
print "Warning, inputs do not contain x key",e
return
"""
try:
u = sum((inputs[uname] for uname in self.unames),[])
except KeyError as e:
print "Warning, inputs do not contain u key",e
#u = [0]*(len(x)/2)
try:
u = vectorops.sub(inputs['qcmd'],inputs['q'])
u = vectorops.div(u,inputs['dt'])
except KeyError as e:
print "Warning, inputs do not contain qcmd key either",e
u = [0]*(len(x)/2)
"""
#self.xlast = x
#return
#do system ID
if self.qlast != None:
self.estimator.add(self.qlast,self.dqlast,u,q,dq)
pass
#update last state
self.qlast = q
self.dqlast = dq
return
