From cf838df461a72455ae5e140a85b5ff0bf9aaed4d Mon Sep 17 00:00:00 2001
From: Mansard <nmansard@laas.fr>
Date: Tue, 6 Sep 2011 14:13:22 +0200
Subject: [PATCH] Added a op point modifier. Should be ported to sot-core.
---
python/meta_task_dyn_6d.py | 22 +++++++++++++++++++++-
1 file changed, 21 insertions(+), 1 deletion(-)
diff --git a/python/meta_task_dyn_6d.py b/python/meta_task_dyn_6d.py
index 44e55f5..070c73b 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)
--
GitLab