diff --git a/python/MetaTask6d.py b/python/MetaTask6d.py new file mode 100644 index 0000000000000000000000000000000000000000..6dd04939dcccc089899fddda250af4f7382e9182 --- /dev/null +++ b/python/MetaTask6d.py @@ -0,0 +1,67 @@ +from dynamic_graph import plug +from dynamic_graph.sot.core import * +from dynamic_graph.sot.core.math_small_entities import Derivator_of_Matrix +from dynamic_graph.sot.dynamics import * +from dynamic_graph.sot.dyninv import * + +class MetaTask6d(object): + name='' + opPoint='' + dyn=0 + derivator=0 + task=0 + feature=0 + featureDes=0 + + def opPointExist(self,opPoint): + sigsP = filter(lambda x: x.getName().split(':')[-1] == opPoint, self.dyn.signals()) + sigsJ = filter(lambda x: x.getName().split(':')[-1] == 'J'+opPoint, self.dyn.signals()) + return len(sigsP)==1 & len(sigsJ)==1 + + def defineDynEntities(self,dyn): + self.dyn=dyn + + def createOpPoint(self,opPoint,opPointRef = 'right-wrist'): + self.opPoint=opPoint + if self.opPointExist(opPoint): return + self.dyn.createOpPoint(opPoint,opPointRef) + + def createFeatures(self): + self.feature = FeaturePoint6d('feature'+self.name) + self.featureDes = FeaturePoint6d('feature'+self.name+'_ref') + self.feature.selec.value = '111111' + self.feature.frame('current') + + def createTask(self): + self.task = TaskDynPD('task'+self.name) + self.task.dt.value = 1e-3 + + def createGain(self): + self.gain = GainAdaptive('gain'+self.name) + self.gain.set(1050,45,125e3) + + def plugEverything(self): + self.feature.sdes.value = self.featureDes.name + plug(self.dyn.signal(self.opPoint),self.feature.signal('position')) + plug(self.dyn.signal('J'+self.opPoint),self.feature.signal('Jq')) + self.task.add(self.feature.name) + plug(self.dyn.velocity,self.task.qdot) + plug(self.task.error,self.gain.error) + plug(self.gain.gain,self.task.controlGain) + + def __init__(self,name,dyn,opPoint,opPointRef='right-wrist'): + self.name=name + self.defineDynEntities(dyn) + self.createOpPoint(opPoint,opPointRef) + self.createFeatures() + self.createTask() + self.createGain() + self.plugEverything() + + @property + def ref(self): + return self.featureDes.position.value + + @ref.setter + def ref(self,m): + self.featureDes.position.value = m diff --git a/python/ThreadInterruptibleLoop.py b/python/ThreadInterruptibleLoop.py new file mode 100644 index 0000000000000000000000000000000000000000..9c96896174704ec9ae4ca9f79d75e406be32bde7 --- /dev/null +++ b/python/ThreadInterruptibleLoop.py @@ -0,0 +1,75 @@ +import signal, threading, time + +class ThreadInterruptibleLoop(threading.Thread): + isQuit=False + isPlay=False + sleepTime=1e-3 + previousHandler = None + isOnce=0 + isRunning=False + iter=0 + + def __init__(self): + threading.Thread.__init__(self) + self.setSigHandler() + + def quit(self): self.isQuit = True + def setPlay(self,mode): + self.isPlay = mode + def play(self): + if not self.isRunning: self.start() + self.isOnce = False + self.setPlay(True) + def pause(self): self.setPlay(False) + def once(self): + self.isOnce=True + self.setPlay(True) + def run(self): + self.isQuit=False + self.isRunning=True + while not self.isQuit: + if self.isPlay: + self.loop() + self.iter+=1 + if self.isOnce: self.pause() + time.sleep(self.sleepTime) + self.isRunning=False + print 'Thread loop will now end.' + + def sigHandler(self,signum, frame): + print 'Catch signal ', signum + signal.signal(signum, self.previousHandler) + self.quit() + def setSigHandler(self): + self.previousHandler = signal.getsignal(signal.SIGINT) + signal.signal(signal.SIGINT, (lambda x,y: self.sigHandler(x,y)) ) + + def start(self): + self.setPlay(True) + threading.Thread.start(self) + def restart(self): + self.join() + self.play() + self.setSigHandler() + threading.Thread.start(self) + def loop(self): + None + + + +# To use the previous class, a 'loop' function has to be define. +# Everything will be embedded by using the decorator below. Just +# use it as: +# >>> @loopInThread +# >>> def Runner(): +# >>> to what you want here +# >>> runner = Runner() +# >>> runner.pause()/play()/quit() ... +def loopInThread( funLoop ): + class ThreadViewer(ThreadInterruptibleLoop): + def __init__(self): + ThreadInterruptibleLoop.__init__(self) +# self.start() + def loop(self): + funLoop() + return ThreadViewer