diff --git a/python/meta_task_dyn_6d.py b/python/meta_task_dyn_6d.py index 44e55f58b2079162abac63c83c4408c5c12b7d18..070c73ba0aff554e40d38cfd5e3f083c0fd8727c 100644 --- a/python/meta_task_dyn_6d.py +++ b/python/meta_task_dyn_6d.py @@ -1,5 +1,5 @@ from dynamic_graph import plug -from dynamic_graph.sot.core import GainAdaptive +from dynamic_graph.sot.core import GainAdaptive, OpPointModifier from dynamic_graph.sot.dyninv import TaskDynPD from dynamic_graph.sot.core.meta_task_6d import MetaTask6d @@ -18,3 +18,23 @@ class MetaTaskDyn6d(MetaTask6d): 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) + 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 + + @property + def opmodif(self): + if not self.opPointModif.activ: return False + else: return self.opPointModif.getTransformation() + + @opmodif.setter + def opmodif(self,m): + if not self.opPointModif.activ: + plug(self.opPointModif.signal('position'),self.feature.position ) + plug(self.opPointModif.signal('jacobian'),self.feature.Jq) + self.opPointModif.activ = True + self.opPointModif.setTransformation(m)