meta_task_6d.py 3.9 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
4
5
from dynamic_graph.sot.core.feature_point6d import FeaturePoint6d
from dynamic_graph.sot.core.gain_adaptive import GainAdaptive
from dynamic_graph.sot.core.op_point_modifier import OpPointModifier
from dynamic_graph.sot.core.sot import Task
Guilhem Saurel's avatar
Guilhem Saurel committed
6

Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
7
8
9
10
11
12

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

Guilhem Saurel's avatar
Guilhem Saurel committed
22

Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
23
class MetaTask6d(object):
Guilhem Saurel's avatar
Guilhem Saurel committed
24
25
26
27
28
29
30
31
    name = ''
    opPoint = ''
    dyn = 0
    task = 0
    feature = 0
    featureDes = 0

    def opPointExist(self, opPoint):
Olivier Stasse's avatar
Olivier Stasse committed
32
33
34
35
36
37
38
39
        sigsP = [
            x for x in self.dyn.signals()
            if x.getName().split(':')[-1] == opPoint
        ]
        sigsJ = [
            x for x in self.dyn.signals()
            if x.getName().split(':')[-1] == 'J' + opPoint
        ]
Guilhem Saurel's avatar
Guilhem Saurel committed
40
41
42
43
44
45
46
47
48
49
50
        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)

51
    def createOpPointModif(self):
Guilhem Saurel's avatar
Guilhem Saurel committed
52
        self.opPointModif = OpPointModifier('opmodif' + self.name)
Olivier Stasse's avatar
Olivier Stasse committed
53
54
55
56
57
58
        plug(
            self.dyn.signal(self.opPoint),
            self.opPointModif.signal('positionIN'))
        plug(
            self.dyn.signal('J' + self.opPoint),
            self.opPointModif.signal('jacobianIN'))
59
        self.opPointModif.activ = False
Guilhem Saurel's avatar
Guilhem Saurel committed
60

Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
61
    def createFeatures(self):
Guilhem Saurel's avatar
Guilhem Saurel committed
62
63
        self.feature = FeaturePoint6d('feature' + self.name)
        self.featureDes = FeaturePoint6d('feature' + self.name + '_ref')
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
64
65
        self.feature.selec.value = '111111'
        self.feature.frame('current')
Guilhem Saurel's avatar
Guilhem Saurel committed
66

Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
67
    def createTask(self):
Guilhem Saurel's avatar
Guilhem Saurel committed
68
69
        self.task = Task('task' + self.name)

Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
70
    def createGain(self):
Guilhem Saurel's avatar
Guilhem Saurel committed
71
72
        self.gain = GainAdaptive('gain' + self.name)
        self.gain.set(0.1, 0.1, 125e3)
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
73
74

    def plugEverything(self):
75
        self.feature.setReference(self.featureDes.name)
Guilhem Saurel's avatar
Guilhem Saurel committed
76
77
        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
78
        self.task.add(self.feature.name)
Guilhem Saurel's avatar
Guilhem Saurel committed
79
80
81
        plug(self.task.error, self.gain.error)
        plug(self.gain.gain, self.task.controlGain)

Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
82
83
84
85
    def keep(self):
        self.feature.position.recompute(self.dyn.position.time)
        self.feature.keep()

Guilhem Saurel's avatar
Guilhem Saurel committed
86
87
    def __init__(self, name, dyn, opPoint, opPointRef='right-wrist'):
        self.name = name
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
88
        self.defineDynEntities(dyn)
Guilhem Saurel's avatar
Guilhem Saurel committed
89
        self.createOpPoint(opPoint, opPointRef)
90
        self.createOpPointModif()
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
91
92
93
94
95
96
97
98
99
100
        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
101
    def ref(self, m):
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
102
        self.featureDes.position.value = m
103
104
105

    @property
    def opmodif(self):
Guilhem Saurel's avatar
Guilhem Saurel committed
106
107
108
109
        if not self.opPointModif.activ:
            return False
        else:
            return self.opPointModif.getTransformation()
110
111

    @opmodif.setter
Guilhem Saurel's avatar
Guilhem Saurel committed
112
113
    def opmodif(self, m):
        if isinstance(m, bool) and not m:
Olivier Stasse's avatar
Olivier Stasse committed
114
115
116
117
            plug(
                self.dyn.signal(self.opPoint), self.feature.signal('position'))
            plug(
                self.dyn.signal('J' + self.opPoint), self.feature.signal('Jq'))
118
119
120
            self.opPointModif.activ = False
        else:
            if not self.opPointModif.activ:
Olivier Stasse's avatar
Olivier Stasse committed
121
122
123
                plug(
                    self.opPointModif.signal('position'),
                    self.feature.position)
Guilhem Saurel's avatar
Guilhem Saurel committed
124
                plug(self.opPointModif.signal('jacobian'), self.feature.Jq)
125
126
            self.opPointModif.setTransformation(m)
            self.opPointModif.activ = True