meta_task_6d.py 3.55 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

def toFlags(arr):
10
11
12
    from warnings import warn
    warn("This function is deprecated. Please, use Flags directly.")
    return Flags(arr)
Guilhem Saurel's avatar
Guilhem Saurel committed
13

Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
14
class MetaTask6d(object):
Guilhem Saurel's avatar
Guilhem Saurel committed
15
16
17
18
19
20
21
22
    name = ''
    opPoint = ''
    dyn = 0
    task = 0
    feature = 0
    featureDes = 0

    def opPointExist(self, opPoint):
Guilhem Saurel's avatar
Guilhem Saurel committed
23
24
        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
25
26
27
28
29
30
31
32
33
34
35
        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)

36
    def createOpPointModif(self):
Guilhem Saurel's avatar
Guilhem Saurel committed
37
        self.opPointModif = OpPointModifier('opmodif' + self.name)
Guilhem Saurel's avatar
Guilhem Saurel committed
38
39
        plug(self.dyn.signal(self.opPoint), self.opPointModif.signal('positionIN'))
        plug(self.dyn.signal('J' + self.opPoint), self.opPointModif.signal('jacobianIN'))
40
        self.opPointModif.activ = False
Guilhem Saurel's avatar
Guilhem Saurel committed
41

Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
42
    def createFeatures(self):
Guilhem Saurel's avatar
Guilhem Saurel committed
43
44
        self.feature = FeaturePoint6d('feature' + self.name)
        self.featureDes = FeaturePoint6d('feature' + self.name + '_ref')
Guilhem Saurel's avatar
Guilhem Saurel committed
45
        self.feature.selec.value = Flags('111111')
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
46
        self.feature.frame('current')
Guilhem Saurel's avatar
Guilhem Saurel committed
47

Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
48
    def createTask(self):
Guilhem Saurel's avatar
Guilhem Saurel committed
49
50
        self.task = Task('task' + self.name)

Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
51
    def createGain(self):
Guilhem Saurel's avatar
Guilhem Saurel committed
52
53
        self.gain = GainAdaptive('gain' + self.name)
        self.gain.set(0.1, 0.1, 125e3)
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
54
55

    def plugEverything(self):
56
        self.feature.setReference(self.featureDes.name)
Guilhem Saurel's avatar
Guilhem Saurel committed
57
58
        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
59
        self.task.add(self.feature.name)
Guilhem Saurel's avatar
Guilhem Saurel committed
60
61
62
        plug(self.task.error, self.gain.error)
        plug(self.gain.gain, self.task.controlGain)

Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
63
64
65
66
    def keep(self):
        self.feature.position.recompute(self.dyn.position.time)
        self.feature.keep()

Guilhem Saurel's avatar
Guilhem Saurel committed
67
68
    def __init__(self, name, dyn, opPoint, opPointRef='right-wrist'):
        self.name = name
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
69
        self.defineDynEntities(dyn)
Guilhem Saurel's avatar
Guilhem Saurel committed
70
        self.createOpPoint(opPoint, opPointRef)
71
        self.createOpPointModif()
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
72
73
74
75
76
77
78
79
80
81
        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
82
    def ref(self, m):
Nicolas Mansard's avatar
IVIGIT.  
Nicolas Mansard committed
83
        self.featureDes.position.value = m
84
85
86

    @property
    def opmodif(self):
Guilhem Saurel's avatar
Guilhem Saurel committed
87
88
89
90
        if not self.opPointModif.activ:
            return False
        else:
            return self.opPointModif.getTransformation()
91
92

    @opmodif.setter
Guilhem Saurel's avatar
Guilhem Saurel committed
93
94
    def opmodif(self, m):
        if isinstance(m, bool) and not m:
Guilhem Saurel's avatar
Guilhem Saurel committed
95
96
            plug(self.dyn.signal(self.opPoint), self.feature.signal('position'))
            plug(self.dyn.signal('J' + self.opPoint), self.feature.signal('Jq'))
97
98
99
            self.opPointModif.activ = False
        else:
            if not self.opPointModif.activ:
Guilhem Saurel's avatar
Guilhem Saurel committed
100
                plug(self.opPointModif.signal('position'), self.feature.position)
Guilhem Saurel's avatar
Guilhem Saurel committed
101
                plug(self.opPointModif.signal('jacobian'), self.feature.Jq)
102
103
            self.opPointModif.setTransformation(m)
            self.opPointModif.activ = True