meta_task_6d.py 3.46 KB
Newer Older
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
from dynamic_graph import plug
from dynamic_graph.sot.core import *

def toFlags(arr):
    """
    Convert an array of boolean to a /flag/ format, type 001010110, in little indian
    (reverse order, first bool of the list will be the [01] of extrem right).
    """
    l=max(arr)+1
    lres=[0]*l
    for i in arr:
        lres[i]=1
    lres.reverse()
    res=''
    for i in lres:
        res+=str(i)
    return res

class MetaTask6d(object):
    name=''
    opPoint=''
    dyn=0
    task=0
    feature=0
    featureDes=0

    def opPointExist(self,opPoint):
        sigsP = filter(lambda x: x.getName().split(':')[-1] == opPoint,
                       self.dyn.signals())
        sigsJ = filter(lambda x: x.getName().split(':')[-1] == 'J'+opPoint,
                       self.dyn.signals())
        return len(sigsP)==1 & len(sigsJ)==1

    def defineDynEntities(self,dyn):
        self.dyn=dyn

    def createOpPoint(self,opPoint,opPointRef = 'right-wrist'):
        self.opPoint=opPoint
        if self.opPointExist(opPoint): return
        self.dyn.createOpPoint(opPoint,opPointRef)
41
42
43
44
45
    def createOpPointModif(self):
        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
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
46
47
48
49
50
51
52
53
54
55
56
57
    def createFeatures(self):
        self.feature    = FeaturePoint6d('feature'+self.name)
        self.featureDes = FeaturePoint6d('feature'+self.name+'_ref')
        self.feature.selec.value = '111111'
        self.feature.frame('current')
    def createTask(self):
        self.task = Task('task'+self.name)
    def createGain(self):
        self.gain = GainAdaptive('gain'+self.name)
        self.gain.set(0.1,0.1,125e3)

    def plugEverything(self):
58
        self.feature.setReference(self.featureDes.name)
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
59
60
61
62
63
64
65
66
67
68
69
70
71
        plug(self.dyn.signal(self.opPoint),self.feature.signal('position'))
        plug(self.dyn.signal('J'+self.opPoint),self.feature.signal('Jq'))
        self.task.add(self.feature.name)
        plug(self.task.error,self.gain.error)
        plug(self.gain.gain,self.task.controlGain)
    def keep(self):
        self.feature.position.recompute(self.dyn.position.time)
        self.feature.keep()

    def __init__(self,name,dyn,opPoint,opPointRef='right-wrist'):
        self.name=name
        self.defineDynEntities(dyn)
        self.createOpPoint(opPoint,opPointRef)
72
        self.createOpPointModif()
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
73
74
75
76
77
78
79
80
81
82
83
84
        self.createFeatures()
        self.createTask()
        self.createGain()
        self.plugEverything()

    @property
    def ref(self):
        return self.featureDes.position.value

    @ref.setter
    def ref(self,m):
        self.featureDes.position.value = m
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102

    @property
    def opmodif(self):
        if not self.opPointModif.activ: return False
        else: return self.opPointModif.getTransformation()

    @opmodif.setter
    def opmodif(self,m):
        if isinstance(m,bool) and m==False:
            plug(self.dyn.signal(self.opPoint),self.feature.signal('position'))
            plug(self.dyn.signal('J'+self.opPoint),self.feature.signal('Jq'))
            self.opPointModif.activ = False
        else:
            if not self.opPointModif.activ:
                plug(self.opPointModif.signal('position'),self.feature.position )
                plug(self.opPointModif.signal('jacobian'),self.feature.Jq)
            self.opPointModif.setTransformation(m)
            self.opPointModif.activ = True