meta_task_pose.py

Go to the documentation of this file.

from dynamic_graph import plug
from dynamic_graph.sot.core import GainAdaptive, Task
from dynamic_graph.sot.core.feature_pose import FeaturePose
 
 
class MetaTaskPose(object):
    def opPointExist(self, opPoint):
        sigsP = filter(
            lambda x: x.getName().split(":")[-1] == opPoint, self.dyndyn.signals()
        )
        sigsJ = filter(
            lambda x: x.getName().split(":")[-1] == "J" + opPoint, self.dyndyn.signals()
        )
        return len(sigsP) == 1 & len(sigsJ) == 1
 
    def defineDynEntities(self, dyn):
        self.dyndyn = dyn
 
    def createOpPoint(self, opPoint, opPointRef="right-wrist"):
        self.opPointopPoint = opPoint
        if self.opPointExistopPointExist(opPoint):
            return
        self.dyndyn.createOpPoint(opPoint, opPointRef)
 
    def createFeature(self):
        self.featurefeature = FeaturePose("feature" + self.namename)
        self.featurefeature.selec.value = "111111"
 
    def createTask(self):
        self.tasktask = Task("task" + self.namename)
 
    def createGain(self):
        self.gaingain = GainAdaptive("gain" + self.namename)
        self.gaingain.set(0.1, 0.1, 125e3)
 
    def plugEverything(self):
        plug(self.dyndyn.signal(self.opPointopPoint), self.featurefeature.oMjb)
        plug(self.dyndyn.signal("J" + self.opPointopPoint), self.featurefeature.jbJjb)
        self.tasktask.add(self.featurefeature.name)
        plug(self.tasktask.error, self.gaingain.error)
        plug(self.gaingain.gain, self.tasktask.controlGain)
 
    def keep(self):
        self.featurefeature.faMfb.recompute(self.dyndyn.position.time)
        self.featurefeature.faMfbDes.value = self.featurefeature.faMfb.value
 
    def __init__(self, name, dyn, opPoint, opPointRef="right-wrist"):
        self.namename = name
        self.defineDynEntitiesdefineDynEntities(dyn)
        self.createOpPointcreateOpPoint(opPoint, opPointRef)
        self.createFeaturecreateFeature()
        self.createTaskcreateTask()
        self.createGaincreateGain()
        self.plugEverythingplugEverything()
 
    @property
    def ref(self):
        return self.featurefeature.faMfbDes.value
 
    @ref.setter
    def ref(self, m):
        self.featurefeature.faMfbDes.value = m