meta_task_6d.py 3.91 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
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
5
from dynamic_graph.sot.core.task import Task
Guilhem Saurel's avatar
Guilhem Saurel committed
6

Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
7
8
9

def toFlags(arr):
    """
Olivier Stasse's avatar
Olivier Stasse committed
10
11
    Convert an array of boolean to a /flag/ format, type 001010110,
    in little indian
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
12
13
    (reverse order, first bool of the list will be the [01] of extrem right).
    """
Guilhem Saurel's avatar
Guilhem Saurel committed
14
    lres = [0] * (max(arr) + 1)
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
15
    for i in arr:
Guilhem Saurel's avatar
Guilhem Saurel committed
16
        lres[i] = 1
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
17
    lres.reverse()
Guilhem Saurel's avatar
Guilhem Saurel committed
18
    res = ''
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
19
    for i in lres:
Guilhem Saurel's avatar
Guilhem Saurel committed
20
        res += str(i)
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
21
22
    return res

Guilhem Saurel's avatar
Guilhem Saurel committed
23

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

    def opPointExist(self, opPoint):
Olivier Stasse's avatar
Olivier Stasse committed
33
34
35
36
37
38
39
40
        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
41
42
43
44
45
46
47
48
49
50
51
        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)

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

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

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

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

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

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

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

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

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