class MetaTaskPose(object): 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 createFeature(self): self.feature = FeaturePose('feature' + self.name) self.feature.selec.value = '111111' def createTask(self): self.task = Task('task' + self.name) def createGain(self): self.gain = GainAdaptive('gain' + self.name) self.gain.set(0.1, 0.1, 125e3) def plugEverything(self): plug(self.dyn.signal(self.opPoint), self.feature.oMjb) plug(self.dyn.signal('J' + self.opPoint), self.feature.jbJjb) self.task.add(self.feature.name) plug(self.task.error, self.gain.error) plug(self.gain.gain, self.task.controlGain) def keep(self): self.feature.faMfb.recompute(self.dyn.position.time) self.feature.faMfbDes.value = self.feature.faMfb.value def __init__(self, name, dyn, opPoint, opPointRef='right-wrist'): self.name = name self.defineDynEntities(dyn) self.createOpPoint(opPoint, opPointRef) self.createFeature() self.createTask() self.createGain() self.plugEverything() @property def ref(self): return self.feature.faMfbDes.value @ref.setter def ref(self, m): self.feature.faMfbDes.value = m
class MetaTaskDyn6d(MetaTask6d): 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.setReference(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,*args): MetaTask6d.__init__(self,*args)
class MetaTaskKine6d(MetaTask6d): def createTask(self): self.task = Task('task' + self.name) def createGain(self): self.gain = GainAdaptive('gain' + self.name) self.gain.set(0.1, 0.1, 125e3) def plugEverything(self): self.feature.setReference(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.task.error, self.gain.error) plug(self.gain.gain, self.task.controlGain) def keep(self): self.feature.position.recompute(self.dyn.position.time) self.feature.keep()
class MetaTaskDyn6dRel(MetaTaskKine6dRel): 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.setReference(self.featureDes.name) plug(self.dyn.signal(self.opPoint),self.feature.signal('position')) plug(self.dyn.signal('J'+self.opPoint),self.feature.signal('Jq')) plug(self.dyn.signal(self.opPointBase),self.feature.signal('positionRef')) plug(self.dyn.signal('J'+self.opPointBase),self.feature.signal('JqRef')) 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,*args): MetaTaskKine6dRel.__init__(self,*args) @property def opmodifBase(self): if not self.opPointModifBase.activ: return False else: return self.opPointModifBase.getTransformation() @opmodifBase.setter def opmodifBase(self,m): if isinstance(m,bool) and m==False: plug(self.dyn.signal(self.opPointBase),self.feature.signal('positionRef')) plug(self.dyn.signal('J'+self.opPointBase),self.feature.signal('JqRef')) self.opPointModifBase.activ = False else: if not self.opPointModifBase.activ: plug(self.opPointModifBase.signal('position'),self.feature.positionRef ) plug(self.opPointModifBase.signal('jacobian'),self.feature.JqRef) self.opPointModifBase.setTransformation(m) self.opPointModifBase.activ = True
class MetaTaskVisualPoint(object): name = '' opPoint = '' dyn = 0 task = 0 feature = 0 featureDes = 0 proj = 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 createOpPointModif(self): self.opPointModif = OpPointModifier('opmodif' + self.name) plug(self.dyn.signal(self.opPoint), self.opPointModif.signal('positionIN')) plug(self.dyn.signal('J' + self.opPoint), self.opPointModif.signal('jacobianIN')) self.opPointModif.activ = False def createFeatures(self): self.feature = FeatureVisualPoint('feature' + self.name) self.featureDes = FeatureVisualPoint('feature' + self.name + '_ref') self.feature.selec.value = '11' def createTask(self): self.task = Task('task' + self.name) def createGain(self): self.gain = GainAdaptive('gain' + self.name) self.gain.set(0.1, 0.1, 125e3) def createProj(self): self.proj = VisualPointProjecter('proj' + self.name) def plugEverything(self): self.feature.setReference(self.featureDes.name) plug(self.dyn.signal(self.opPoint), self.proj.signal('transfo')) plug(self.proj.signal('point2D'), self.feature.signal('xy')) plug(self.proj.signal('depth'), self.feature.signal('Z')) plug(self.dyn.signal('J' + self.opPoint), self.feature.signal('Jq')) self.task.add(self.feature.name) 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.createOpPointModif() self.createFeatures() self.createTask() self.createGain() self.createProj() self.plugEverything() @property def ref(self): return self.featureDes.xy.value @ref.setter def ref(self, m): self.featureDes.xy.value = m @property def target(self): return self.proj.point3D @target.setter def target(self, m): self.proj.point3D.value = m @property def opmodif(self): if not self.opPointModif.activ: return False else: return self.opPointModif.getTransformation() @opmodif.setter def opmodif(self, m): if isinstance(m, bool) and not m: plug(self.dyn.signal(self.opPoint), self.proj.signal('transfo')) plug(self.dyn.signal('J' + self.opPoint), self.feature.signal('Jq')) self.opPointModif.activ = False else: if not self.opPointModif.activ: plug(self.opPointModif.signal('position'), self.proj.signal('transfo')) plug(self.opPointModif.signal('jacobian'), self.feature.signal('Jq')) self.opPointModif.setTransformation(m) self.opPointModif.activ = True def goto3D(self, point3D, gain=None, ref2D=None, selec=None): self.target = point3D if ref2D is not None: self.ref = ref2D if selec is not None: self.feature.selec.value = selec setGain(self.gain, gain)
class MetaTask6d(object): name = '' opPoint = '' dyn = 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 createOpPointModif(self): self.opPointModif = OpPointModifier('opmodif' + self.name) plug(self.dyn.signal(self.opPoint), self.opPointModif.signal('positionIN')) plug(self.dyn.signal('J' + self.opPoint), self.opPointModif.signal('jacobianIN')) self.opPointModif.activ = False 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 = Task('task' + self.name) def createGain(self): self.gain = GainAdaptive('gain' + self.name) self.gain.set(0.1, 0.1, 125e3) def plugEverything(self): self.feature.setReference(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.task.error, self.gain.error) plug(self.gain.gain, self.task.controlGain) def keep(self): self.feature.position.recompute(self.dyn.position.time) self.feature.keep() def __init__(self, name, dyn, opPoint, opPointRef='right-wrist'): self.name = name self.defineDynEntities(dyn) self.createOpPoint(opPoint, opPointRef) self.createOpPointModif() 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 @property def opmodif(self): if not self.opPointModif.activ: return False else: return self.opPointModif.getTransformation() @opmodif.setter def opmodif(self, m): if isinstance(m, bool) and not m: plug(self.dyn.signal(self.opPoint), self.feature.signal('position')) plug(self.dyn.signal('J' + self.opPoint), self.feature.signal('Jq')) self.opPointModif.activ = False else: if not self.opPointModif.activ: plug(self.opPointModif.signal('position'), self.feature.position) plug(self.opPointModif.signal('jacobian'), self.feature.Jq) self.opPointModif.setTransformation(m) self.opPointModif.activ = True