def update_input(self):
self.raw_reading = self.get_reading()
"""
if self.last_reading:
if any(abs(b-a)>2 for (a,b) in zip(self.last_reading,self.raw_reading)):
print "New reading..."
self.last_reading = self.raw_reading
else:
self.last_reading = self.raw_reading
"""
self.filtered_reading = self.filter_reading(self.raw_reading)
q = self.calibration.readingToConfig(self.filtered_reading)
#map to Klamp't configs
q = settings.arduinoToKlampt(q)
qmin, qmax = self.robot.getJointLimits()
for i, (qi, a, b) in enumerate(zip(q, qmin, qmax)):
buf = 1e-7 * (b - a)
q[i] = min(b - buf, max(qi, a + buf))
"""
if self.qfiltered!=None:
if any(abs(a-b)>1e-4 for (a,b) in zip(q,self.qfiltered)):
print "New input"
"""
self.qfiltered = q
self.controller.setMilestone(q)
def update_input(self):
self.raw_reading = self.get_reading()
"""
if self.last_reading:
if any(abs(b-a)>2 for (a,b) in zip(self.last_reading,self.raw_reading)):
print "New reading..."
self.last_reading = self.raw_reading
else:
self.last_reading = self.raw_reading
"""
self.filtered_reading = self.filter_reading(self.raw_reading)
q = self.calibration.readingToConfig(self.filtered_reading)
#map to Klamp't configs
q = settings.arduinoToKlampt(q)
qmin,qmax = self.robot.getJointLimits()
for i,(qi,a,b) in enumerate(zip(q,qmin,qmax)):
buf = 1e-7*(b-a)
q[i] = min(b-buf,max(qi,a+buf))
"""
if self.qfiltered!=None:
if any(abs(a-b)>1e-4 for (a,b) in zip(q,self.qfiltered)):
print "New input"
"""
self.qfiltered = q
self.controller.setMilestone(q)
def setConfig(self, q):
#default: direct control
#self.robot.setConfig(q)
#Convert to Klamp't configs
q = settings.arduinoToKlampt(q)
self.robot.setConfig(q)
return
def setConfig(self,q):
#default: direct control
#self.robot.setConfig(q)
#Convert to Klamp't configs
q = settings.arduinoToKlampt(q)
self.robot.setConfig(q)
return
rate = 30
host = ''
port = 3456
backlog = 1
print "***** Setting up socket server on port %d. *****"%(port,)
publisher = messagePublisher.Publisher((host,port),backlog)
print "***** Waiting for clients... *****"""
publisher.accept()
try:
print "***** Streaming data at %sHz... *****"%(str(rate),)
while True:
reading = filter.process(get_reading())
q = calibration.readingToConfig(reading)
#map from arduino to Klamp'
q = settings.arduinoToKlampt(q)
msg = {"type":"config","data":q}
msgstr = json.dumps(msg)
#print msgstr,'length',len(msgstr)
try:
publisher.write(msgstr)
except IOError:
print "***** Client error, killing connection. *****"
publisher.closeClient()
print "***** Waiting for clients... *****"
publisher.accept()
print "***** Streaming data at %sHz... *****"%(str(rate),)
time.sleep(1.0/rate)
except KeyboardInterrupt,SystemExit:
publisher.closeServer()
