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

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

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

Guilhem Saurel's avatar
Guilhem Saurel committed
24

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

    def opPointExist(self, opPoint):
Guilhem Saurel's avatar
Guilhem Saurel committed
34
35
        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
36
37
38
39
40
41
42
43
44
45
46
        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)

47
    def createOpPointModif(self):
Guilhem Saurel's avatar
Guilhem Saurel committed
48
        self.opPointModif = OpPointModifier('opmodif' + self.name)
Guilhem Saurel's avatar
Guilhem Saurel committed
49
50
        plug(self.dyn.signal(self.opPoint), self.opPointModif.signal('positionIN'))
        plug(self.dyn.signal('J' + self.opPoint), self.opPointModif.signal('jacobianIN'))
51
        self.opPointModif.activ = False
Guilhem Saurel's avatar
Guilhem Saurel committed
52

Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
53
    def createFeatures(self):
Guilhem Saurel's avatar
Guilhem Saurel committed
54
55
        self.feature = FeaturePoint6d('feature' + self.name)
        self.featureDes = FeaturePoint6d('feature' + self.name + '_ref')
Guilhem Saurel's avatar
Guilhem Saurel committed
56
        self.feature.selec.value = Flags('111111')
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
57
        self.feature.frame('current')
Guilhem Saurel's avatar
Guilhem Saurel committed
58

Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
59
    def createTask(self):
Guilhem Saurel's avatar
Guilhem Saurel committed
60
61
        self.task = Task('task' + self.name)

Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
62
    def createGain(self):
Guilhem Saurel's avatar
Guilhem Saurel committed
63
64
        self.gain = GainAdaptive('gain' + self.name)
        self.gain.set(0.1, 0.1, 125e3)
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
65
66

    def plugEverything(self):
67
        self.feature.setReference(self.featureDes.name)
Guilhem Saurel's avatar
Guilhem Saurel committed
68
69
        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
70
        self.task.add(self.feature.name)
Guilhem Saurel's avatar
Guilhem Saurel committed
71
72
73
        plug(self.task.error, self.gain.error)
        plug(self.gain.gain, self.task.controlGain)

Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
74
75
76
77
    def keep(self):
        self.feature.position.recompute(self.dyn.position.time)
        self.feature.keep()

Guilhem Saurel's avatar
Guilhem Saurel committed
78
79
    def __init__(self, name, dyn, opPoint, opPointRef='right-wrist'):
        self.name = name
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
80
        self.defineDynEntities(dyn)
Guilhem Saurel's avatar
Guilhem Saurel committed
81
        self.createOpPoint(opPoint, opPointRef)
82
        self.createOpPointModif()
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
83
84
85
86
87
88
89
90
91
92
        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
93
    def ref(self, m):
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
94
        self.featureDes.position.value = m
95
96
97

    @property
    def opmodif(self):
Guilhem Saurel's avatar
Guilhem Saurel committed
98
99
100
101
        if not self.opPointModif.activ:
            return False
        else:
            return self.opPointModif.getTransformation()
102
103

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