def __init__(self, world):
GLRealtimeProgram.__init__(self, "Arduino control")
self.world = world
self.sim = Simulator(world)
self.robot = world.robot(0)
self.controller = self.sim.getController(0)
if not synthetic:
self.readthread = ReadArduinoThread()
self.readthread.start()
self.readthread.wait_for_data()
self.calibration = Calibration()
self.calibration.load()
self.filter = CompositeFilter(
[DeadbandFilter(), ExponentialFilter(0.5)])
self.last_reading = None
self.qfiltered = None
def __init__(self,world):
GLRealtimeProgram.__init__(self,"Arduino control")
self.world = world
self.sim = Simulator(world)
self.robot = world.robot(0)
self.controller = self.sim.getController(0)
if not synthetic:
self.readthread = ReadArduinoThread()
self.readthread.start()
self.readthread.wait_for_data()
self.calibration = Calibration()
self.calibration.load()
self.filter = CompositeFilter([DeadbandFilter(),ExponentialFilter(0.5)])
self.last_reading = None
self.qfiltered = None
import messagePublisher
import settings
from filters import *
from klampt import vectorops
import math
import time
import json
import sys
print """syntheticSerialRelay.py: sends wiggling messages to the
Klamp't objective function server on localhost:3456. Uses the calibration
in arduinoCalibrate.txt to map to Klamp't configurations. Sends a "config"
type objective.
"""
calibration = Calibration()
calibration.load()
print "***** Using a deadband filter with exponential smoothing *****"
filter = CompositeFilter([DeadbandFilter(),ExponentialFilter(0.5)])
print "***** Running synthetic Arduino stream... *****"
def get_reading():
#mags = [512,512,512,512,512]
mags = [256,256,256,256,256]
periods = [20,13,7,6,22]
return [512 + mag*math.sin(time.time()/period) for mag,period in zip(mags,periods)]
rate = 30
host = ''
class GLControlGUI(GLRealtimeProgram):
def __init__(self, world):
GLRealtimeProgram.__init__(self, "Arduino control")
self.world = world
self.sim = Simulator(world)
self.robot = world.robot(0)
self.controller = self.sim.getController(0)
if not synthetic:
self.readthread = ReadArduinoThread()
self.readthread.start()
self.readthread.wait_for_data()
self.calibration = Calibration()
self.calibration.load()
self.filter = CompositeFilter(
[DeadbandFilter(), ExponentialFilter(0.5)])
self.last_reading = None
self.qfiltered = None
#null filter
#self.filter = ExponentialFilter(1)
def get_reading(self):
if synthetic:
return [1024, (time.time() * 1000) % 1024, 1024, 1024, 1024]
(cnt, timestamp, values) = self.readthread.get_data()
t = time.time()
#if t - timestamp > 0.05:
print "Read thread delay", t - timestamp
return [float(v) for v in values]
def filter_reading(self, reading):
return self.filter.process(reading)
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 display(self):
self.sim.updateWorld()
self.world.drawGL()
#draw commanded configurations
if self.qfiltered != None:
glEnable(GL_BLEND)
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE,
[1, 0.5, 0, 0.5])
self.world.robot(0).setConfig(self.qfiltered)
self.world.robot(0).drawGL(False)
glDisable(GL_BLEND)
def idle(self):
if not hasattr(self, 'iteration'):
self.iteration = 0
if not hasattr(self, 'tlast'):
self.tlast = time.time()
self.iteration += 1
if self.iteration % 30 == 0:
t = time.time()
print "Simulation time: %f, clock time: %g\n" % (self.dt * 30,
t - self.tlast)
self.tlast = t
self.update_input()
self.sim.simulate(self.dt)
if record:
recordfile.write('%f,' % (self.sim.getTime(), ))
recordfile.write(','.join(
str(x)
for x in self.calibration.readingToConfig(self.raw_reading)))
recordfile.write(',')
recordfile.write(','.join(
str(x) for x in self.calibration.readingToConfig(
self.filtered_reading)))
recordfile.write(',')
recordfile.write(','.join(
str(x) for x in self.controller.getCommandedConfig()[1:6]))
recordfile.write('\n')
glutPostRedisplay()
class GLControlGUI(GLRealtimeProgram):
def __init__(self,world):
GLRealtimeProgram.__init__(self,"Arduino control")
self.world = world
self.sim = Simulator(world)
self.robot = world.robot(0)
self.controller = self.sim.getController(0)
if not synthetic:
self.readthread = ReadArduinoThread()
self.readthread.start()
self.readthread.wait_for_data()
self.calibration = Calibration()
self.calibration.load()
self.filter = CompositeFilter([DeadbandFilter(),ExponentialFilter(0.5)])
self.last_reading = None
self.qfiltered = None
#null filter
#self.filter = ExponentialFilter(1)
def get_reading(self):
if synthetic:
return [1024,(time.time()*1000)%1024,1024,1024,1024]
(cnt,timestamp,values)=self.readthread.get_data()
t = time.time()
#if t - timestamp > 0.05:
print "Read thread delay",t-timestamp
return [float(v) for v in values]
def filter_reading(self,reading):
return self.filter.process(reading)
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 display(self):
self.sim.updateWorld()
self.world.drawGL()
#draw commanded configurations
if self.qfiltered!=None:
glEnable(GL_BLEND)
glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA)
glMaterialfv(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE,[1,0.5,0,0.5])
self.world.robot(0).setConfig(self.qfiltered)
self.world.robot(0).drawGL(False)
glDisable(GL_BLEND)
def idle(self):
if not hasattr(self,'iteration'):
self.iteration=0
if not hasattr(self,'tlast'):
self.tlast=time.time()
self.iteration += 1
if self.iteration % 30 == 0:
t = time.time()
print "Simulation time: %f, clock time: %g\n"%(self.dt*30,t-self.tlast)
self.tlast = t
self.update_input()
self.sim.simulate(self.dt)
if record:
recordfile.write('%f,'%(self.sim.getTime(),))
recordfile.write(','.join(str(x) for x in self.calibration.readingToConfig(self.raw_reading)))
recordfile.write(',')
recordfile.write(','.join(str(x) for x in self.calibration.readingToConfig(self.filtered_reading)))
recordfile.write(',')
recordfile.write(','.join(str(x) for x in self.controller.getCommandedConfig()[1:6]))
recordfile.write('\n')
glutPostRedisplay()
