Skip to content
Snippets Groups Projects
Commit cf838df4 authored by Nicolas Mansard's avatar Nicolas Mansard Committed by nmansard
Browse files

Added a op point modifier. Should be ported to sot-core.

parent 7232e565
No related branches found
No related tags found
No related merge requests found
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)
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment