meta_task_config.py

Go to the documentation of this file.

from dynamic_graph import plug
from dynamic_graph.sot.core import FeatureGeneric, GainAdaptive, Task
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from dynamic_graph.sot.core.meta_task_6d import toFlags
from numpy import identity
 
 
class MetaTaskConfig(object):
    def __init__(self, dyn, config=None, name="config"):
        self.dyndyn = dyn
        self.namename = name
        self.configconfig = config
 
        self.featurefeature = FeatureGeneric("feature" + name)
        self.featureDesfeatureDes = FeatureGeneric("featureDes" + name)
        self.gaingain = GainAdaptive("gain" + name)
 
        plug(dyn.position, self.featurefeature.errorIN)
        robotDim = dyn.getDimension()
        self.featurefeature.jacobianIN.value = matrixToTuple(identity(robotDim))
        self.featurefeature.setReference(self.featureDesfeatureDes.name)
        if config is not None:
            self.featurefeature.selec.value = toFlags(self.configconfig)
 
    def plugTask(self):
        self.task.add(self.featurefeature.name)
        plug(self.task.error, self.gaingain.error)
        plug(self.gaingain.gain, self.task.controlGain)
 
    @property
    def ref(self):
        return self.featureDesfeatureDes.errorIN.value
 
    @ref.setter
    def ref(self, v):
        self.featureDesfeatureDes.errorIN.value = v
 
 
class MetaTaskKineConfig(MetaTaskConfig):
    def __init__(self, dyn, config=None, name="config"):
        MetaTaskConfig.__init__(self, dyn, config, name)
        self.tasktask = Task("task" + name)
        self.plugTaskplugTask()