meta_task_joint.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.operator import Selec_of_vector
from numpy import identity
 
 
class MetaTaskJoint(object):
    def __init__(self, dyn, joint, name=None):
        if name is None:
            name = "joint" + str(joint)
        self.dyndyn = dyn
        self.namename = name
        self.jointjoint = joint
 
        self.featurefeature = FeatureGeneric("feature" + name)
        self.featureDesfeatureDes = FeatureGeneric("featureDes" + name)
        self.gaingain = GainAdaptive("gain" + name)
 
        self.selecselec = Selec_of_vector("selec" + name)
        self.selecselec.selec(joint, joint + 1)
        plug(dyn.position, self.selecselec.sin)
        plug(self.selecselec.sout, self.featurefeature.errorIN)
 
        robotDim = len(dyn.position.value)
        Id = identity(robotDim)
        J = Id[joint : joint + 1]
        self.featurefeature.jacobianIN.value = matrixToTuple(J)
        self.featurefeature.setReference(self.featureDesfeatureDes.name)
 
    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 MetaTaskKineJoint(MetaTaskJoint):
    def __init__(self, dyn, joint, name=None):
        MetaTaskJoint.__init__(self, dyn, joint, name)
        self.tasktask = Task("task" + self.namename)
        self.plugTaskplugTask()