meta_task_6d.py 3.57 KB
Newer Older
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
1
from dynamic_graph import plug
Guilhem Saurel's avatar
Guilhem Saurel committed
2
3
from dynamic_graph.sot.core import FeaturePoint6d, GainAdaptive, OpPointModifier, Task

Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
4
5
6
7
8
9

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).
    """
Guilhem Saurel's avatar
Guilhem Saurel committed
10
    lres = [0] * (max(arr) + 1)
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
11
    for i in arr:
Guilhem Saurel's avatar
Guilhem Saurel committed
12
        lres[i] = 1
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
13
    lres.reverse()
Guilhem Saurel's avatar
Guilhem Saurel committed
14
    res = ''
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
15
    for i in lres:
Guilhem Saurel's avatar
Guilhem Saurel committed
16
        res += str(i)
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
17
18
    return res

Guilhem Saurel's avatar
Guilhem Saurel committed
19

Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
20
class MetaTask6d(object):
Guilhem Saurel's avatar
Guilhem Saurel committed
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
    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)

42
    def createOpPointModif(self):
Guilhem Saurel's avatar
Guilhem Saurel committed
43
44
45
        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'))
46
        self.opPointModif.activ = False
Guilhem Saurel's avatar
Guilhem Saurel committed
47

Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
48
    def createFeatures(self):
Guilhem Saurel's avatar
Guilhem Saurel committed
49
50
        self.feature = FeaturePoint6d('feature' + self.name)
        self.featureDes = FeaturePoint6d('feature' + self.name + '_ref')
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
51
52
        self.feature.selec.value = '111111'
        self.feature.frame('current')
Guilhem Saurel's avatar
Guilhem Saurel committed
53

Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
54
    def createTask(self):
Guilhem Saurel's avatar
Guilhem Saurel committed
55
56
        self.task = Task('task' + self.name)

Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
57
    def createGain(self):
Guilhem Saurel's avatar
Guilhem Saurel committed
58
59
        self.gain = GainAdaptive('gain' + self.name)
        self.gain.set(0.1, 0.1, 125e3)
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
60
61

    def plugEverything(self):
62
        self.feature.setReference(self.featureDes.name)
Guilhem Saurel's avatar
Guilhem Saurel committed
63
64
        plug(self.dyn.signal(self.opPoint), self.feature.signal('position'))
        plug(self.dyn.signal('J' + self.opPoint), self.feature.signal('Jq'))
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
65
        self.task.add(self.feature.name)
Guilhem Saurel's avatar
Guilhem Saurel committed
66
67
68
        plug(self.task.error, self.gain.error)
        plug(self.gain.gain, self.task.controlGain)

Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
69
70
71
72
    def keep(self):
        self.feature.position.recompute(self.dyn.position.time)
        self.feature.keep()

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

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

    @ref.setter
Guilhem Saurel's avatar
Guilhem Saurel committed
88
    def ref(self, m):
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
89
        self.featureDes.position.value = m
90
91
92

    @property
    def opmodif(self):
Guilhem Saurel's avatar
Guilhem Saurel committed
93
94
95
96
        if not self.opPointModif.activ:
            return False
        else:
            return self.opPointModif.getTransformation()
97
98

    @opmodif.setter
Guilhem Saurel's avatar
Guilhem Saurel committed
99
100
101
102
    def opmodif(self, m):
        if isinstance(m, bool) and not m:
            plug(self.dyn.signal(self.opPoint), self.feature.signal('position'))
            plug(self.dyn.signal('J' + self.opPoint), self.feature.signal('Jq'))
103
104
105
            self.opPointModif.activ = False
        else:
            if not self.opPointModif.activ:
Guilhem Saurel's avatar
Guilhem Saurel committed
106
107
                plug(self.opPointModif.signal('position'), self.feature.position)
                plug(self.opPointModif.signal('jacobian'), self.feature.Jq)
108
109
            self.opPointModif.setTransformation(m)
            self.opPointModif.activ = True