meta_tasks_kine_relative.py 5.45 KB
Newer Older
1
from dynamic_graph import plug
Guilhem Saurel's avatar
Guilhem Saurel committed
2
3
from dynamic_graph.sot.core import Flags
from dynamic_graph.sot.core.feature_point6d_relative import FeaturePoint6dRelative
Guilhem Saurel's avatar
Guilhem Saurel committed
4
5
6
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from dynamic_graph.sot.core.meta_task_6d import MetaTask6d, toFlags
from dynamic_graph.sot.core.meta_tasks import generic6dReference, setGain
Guilhem Saurel's avatar
Guilhem Saurel committed
7
from dynamic_graph.sot.core.op_point_modifier import OpPointModifier
Guilhem Saurel's avatar
Guilhem Saurel committed
8
9
10
11
12
13
14
15
16
17
18
19
20
21


class MetaTaskKine6dRel(MetaTask6d):

    opPointBase = ''

    def createOpPointBase(self, opPointBase, opPointRefBase='left-wrist'):
        self.opPointBase = opPointBase
        if self.opPointExist(opPointBase):
            return
        self.dyn.createOpPoint(opPointBase, opPointRefBase)

    def createOpPointModifBase(self):
        self.opPointModifBase = OpPointModifier('opmodifBase' + self.name)
Guilhem Saurel's avatar
Guilhem Saurel committed
22
23
        plug(self.dyn.signal(self.opPointBase), self.opPointModifBase.signal('positionIN'))
        plug(self.dyn.signal('J' + self.opPointBase), self.opPointModifBase.signal('jacobianIN'))
Guilhem Saurel's avatar
Guilhem Saurel committed
24
25
26
27
        self.opPointModifBase.activ = False

    def createFeatures(self):
        self.feature = FeaturePoint6dRelative('featureRel' + self.name)
Guilhem Saurel's avatar
Guilhem Saurel committed
28
29
        self.featureDes = FeaturePoint6dRelative('featureRel' + self.name + '_ref')
        self.feature.selec.value = Flags('111111')
Guilhem Saurel's avatar
Guilhem Saurel committed
30
31
32
33
34
35
        self.feature.frame('current')

    def plugEverything(self):
        self.feature.setReference(self.featureDes.name)
        plug(self.dyn.signal(self.opPoint), self.feature.signal('position'))
        plug(self.dyn.signal('J' + self.opPoint), self.feature.signal('Jq'))
Guilhem Saurel's avatar
Guilhem Saurel committed
36
37
        plug(self.dyn.signal(self.opPointBase), self.feature.signal('positionRef'))
        plug(self.dyn.signal('J' + self.opPointBase), self.feature.signal('JqRef'))
Guilhem Saurel's avatar
Guilhem Saurel committed
38
39
40
41
42
43
44
45
        self.task.add(self.feature.name)
        plug(self.task.error, self.gain.error)
        plug(self.gain.gain, self.task.controlGain)

    def keep(self):
        self.feature.position.recompute(self.dyn.position.time)
        self.feature.keep()

Guilhem Saurel's avatar
Guilhem Saurel committed
46
    def __init__(self, name, dyn, opPoint, opPointBase, opPointRef='right-wrist', opPointRefBase='left-wrist'):
Guilhem Saurel's avatar
Guilhem Saurel committed
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
        self.name = name
        self.defineDynEntities(dyn)
        self.createOpPoint(opPoint, opPointRef)
        self.createOpPointBase(opPointBase, opPointRefBase)
        self.createOpPointModif()
        self.createOpPointModifBase()
        self.createFeatures()
        self.createTask()
        self.createGain()
        self.plugEverything()

    @property
    def opmodifBase(self):
        if not self.opPointModifBase.activ:
            return False
        else:
            return self.opPointModifBase.getTransformation()

    @opmodifBase.setter
    def opmodifBase(self, m):
        if isinstance(m, bool) and not m:
Guilhem Saurel's avatar
Guilhem Saurel committed
68
69
            plug(self.dyn.signal(self.opPointBase), self.feature.signal('positionRef'))
            plug(self.dyn.signal('J' + self.opPointBase), self.feature.signal('JqRef'))
Guilhem Saurel's avatar
Guilhem Saurel committed
70
71
72
            self.opPointModifBase.activ = False
        else:
            if not self.opPointModifBase.activ:
Guilhem Saurel's avatar
Guilhem Saurel committed
73
74
                plug(self.opPointModifBase.signal('position'), self.feature.positionRef)
                plug(self.opPointModifBase.signal('jacobian'), self.feature.JqRef)
Guilhem Saurel's avatar
Guilhem Saurel committed
75
76
            self.opPointModifBase.setTransformation(m)
            self.opPointModifBase.activ = True
77
78


Olivier Stasse's avatar
Olivier Stasse committed
79
# --- HELPER FUNCTIONS --------------------------------------------------------
80

Guilhem Saurel's avatar
Guilhem Saurel committed
81
82
83
84
85
86

def goto6dRel(task, position, positionRef, gain=None, resetJacobian=True):
    M = generic6dReference(position)
    MRef = generic6dReference(positionRef)
    task.featureDes.position.value = matrixToTuple(M)
    task.featureDes.positionRef.value = matrixToTuple(MRef)
Guilhem Saurel's avatar
Guilhem Saurel committed
87
    task.feature.selec.value = Flags("111111")
Guilhem Saurel's avatar
Guilhem Saurel committed
88
    setGain(task.gain, gain)
Guilhem Saurel's avatar
Guilhem Saurel committed
89
    if 'resetJacobianDerivative' in task.task.__class__.__dict__.keys() and resetJacobian:
Guilhem Saurel's avatar
Guilhem Saurel committed
90
91
92
        task.task.resetJacobianDerivative()


Guilhem Saurel's avatar
Guilhem Saurel committed
93
def gotoNdRel(task, position, positionRef, selec=None, gain=None, resetJacobian=True):
Guilhem Saurel's avatar
Guilhem Saurel committed
94
95
96
97
98
99
100
101
102
103
    M = generic6dReference(position)
    MRef = generic6dReference(positionRef)
    if selec is not None:
        if isinstance(selec, str):
            task.feature.selec.value = selec
        else:
            task.feature.selec.value = toFlags(selec)
    task.featureDes.position.value = matrixToTuple(M)
    task.featureDes.positionRef.value = matrixToTuple(MRef)
    setGain(task.gain, gain)
Guilhem Saurel's avatar
Guilhem Saurel committed
104
    if 'resetJacobianDerivative' in task.task.__class__.__dict__.keys() and resetJacobian:
Guilhem Saurel's avatar
Guilhem Saurel committed
105
        task.task.resetJacobianDerivative()
106
107
108
109
110
111
112
113


"""

Documentation

Inherited from MetaTask6d.

Olivier Stasse's avatar
Olivier Stasse committed
114
115
116
117
The aim of this MetaTask is to give a simple and immediate interface to
implement a relative task between two
operational points of the robot. The new variable "opPointBase" represents
in fact the second operational point (the
Guilhem Saurel's avatar
Guilhem Saurel committed
118
first is inherited from the father class).
119

Olivier Stasse's avatar
Olivier Stasse committed
120
121
122
123
It's been decided to reuse (so not to redefine) all methodes from MetaTask6d
related to the opPoint to implement the
behaviour of one of the two points (called "Other" from now on) and to
reimplement in a intuitive way the same
Guilhem Saurel's avatar
Guilhem Saurel committed
124
functions for the second point ("Ref").
125
126
127
128


Utilization

Olivier Stasse's avatar
Olivier Stasse committed
129
130
131
132
133
134
It should be noticed that both feature and reference are defined as a
couple of signals, while normally it would be
enough define the reference as one signal that represents the diplacement
between the two positions. Nevertheless this
redundant approach allows to a very intuitive and safe usage of the class
because the references can be set just using
Guilhem Saurel's avatar
Guilhem Saurel committed
135
the current position of the two operational points.
136
137
138
For this reason all the goTo functions have been redefined.

"""