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

Guilhem Saurel's avatar
flake8    
Guilhem Saurel committed
14

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

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

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

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

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

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

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

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

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

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

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