tools.py 12.2 KB
Newer Older
Joseph Mirabel's avatar
Joseph Mirabel committed
1
from hpp import Transform
2
from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d, MetaTaskKineCom
Joseph Mirabel's avatar
Joseph Mirabel committed
3
4
from dynamic_graph.sot.core.meta_tasks_kine_relative import MetaTaskKine6dRel
from dynamic_graph.sot.core.meta_task_posture import MetaTaskKinePosture
Joseph Mirabel's avatar
Joseph Mirabel committed
5
from dynamic_graph.sot.core.meta_tasks import setGain
Joseph Mirabel's avatar
Joseph Mirabel committed
6
from dynamic_graph.sot.core import FeaturePosture
Joseph Mirabel's avatar
Joseph Mirabel committed
7
8

def parseHppName (hppjointname):
9
    if hppjointname == "universe": return "", "universe"
Joseph Mirabel's avatar
Joseph Mirabel committed
10
11
12
13
14
15
16
17
18
19
    return hppjointname.split('/', 1)

def transformToMatrix (T):
    from numpy import eye
    M = eye (4)
    M[:3,:3] = T.quaternion.toRotationMatrix()
    M[:3,3] = T.translation
    return M

class Manifold(object):
Joseph Mirabel's avatar
Joseph Mirabel committed
20
21
    sep = "___"

Joseph Mirabel's avatar
Joseph Mirabel committed
22
    def __init__ (self, tasks = [], constraints = [], topics = {}):
Joseph Mirabel's avatar
Joseph Mirabel committed
23
24
25
        self.tasks = list(tasks)
        self.constraints = list(constraints)
        self.topics = dict(topics)
Joseph Mirabel's avatar
Joseph Mirabel committed
26
27

    def __add__ (self, other):
Joseph Mirabel's avatar
Joseph Mirabel committed
28
        res = Manifold(list(self.tasks), list(self.constraints), dict(self.topics))
Joseph Mirabel's avatar
Joseph Mirabel committed
29
30
31
32
33
34
35
36
37
38
39
40
        res += other
        return res

    def __iadd__ (self, other):
        self.tasks += other.tasks
        self.constraints += other.constraints
        for k,v in other.topics.items():
            if self.topics.has_key(k):
                a = self.topics[k]
                assert a["type"] == v["type"]
                if a.has_key('topic'): assert a["topic"] == v["topic"]
                else: assert a["handler"] == v["handler"]
Joseph Mirabel's avatar
Joseph Mirabel committed
41
42
                a["signalGetters"] += list(v["signalGetters"])
                # print k, "has", len(a["signalGetters"]), "signals"
Joseph Mirabel's avatar
Joseph Mirabel committed
43
            else:
Joseph Mirabel's avatar
Joseph Mirabel committed
44
                self.topics[k] = dict(v)
Joseph Mirabel's avatar
Joseph Mirabel committed
45
46
47
48
49
50
51
52
53
        return self

    def pushTo (self, sot):
        for t in self.tasks:
            sot.push(t.name)

class Posture(Manifold):
    def __init__ (self, name, sotrobot):
        super(Posture, self).__init__()
Joseph Mirabel's avatar
Joseph Mirabel committed
54
55
56
57
58
59
60
61
62
63
64
65
        from dynamic_graph.sot.core import Task, FeatureGeneric, GainAdaptive, Selec_of_vector
        from dynamic_graph.sot.core.matrix_util import matrixToTuple
        from dynamic_graph import plug
        from numpy import identity, hstack, zeros

        n = Posture.sep + name
        self.tp = Task ('task' + n)
        self.tp.dyn = sotrobot.dynamic
        self.tp.feature = FeatureGeneric('feature_'+n)
        self.tp.featureDes = FeatureGeneric('feature_des_'+n)
        self.tp.gain = GainAdaptive("gain_"+n)
        robotDim = sotrobot.dynamic.getDimension()
66
        self.tp.feature.jacobianIN.value = matrixToTuple( identity(robotDim) )
Joseph Mirabel's avatar
Joseph Mirabel committed
67
68
69
70
        self.tp.feature.setReference(self.tp.featureDes.name)
        self.tp.add(self.tp.feature.name)

        # Connects the dynamics to the current feature of the posture task
71
        plug(sotrobot.dynamic.position, self.tp.feature.errorIN)
Joseph Mirabel's avatar
Joseph Mirabel committed
72

73
74
        self.tp.setWithDerivative (True)

Joseph Mirabel's avatar
Joseph Mirabel committed
75
        # Set the gain of the posture task
Joseph Mirabel's avatar
Joseph Mirabel committed
76
        setGain(self.tp.gain,10)
Joseph Mirabel's avatar
Joseph Mirabel committed
77
78
79
        plug(self.tp.gain.gain, self.tp.controlGain)
        plug(self.tp.error, self.tp.gain.error)
        self.tasks = [ self.tp ]
Joseph Mirabel's avatar
Joseph Mirabel committed
80
81
82
        self.topics = {
                    name: {
                        "type": "vector",
Joseph Mirabel's avatar
Joseph Mirabel committed
83
                        "topic": "/hpp/target/position",
Joseph Mirabel's avatar
Joseph Mirabel committed
84
85
86
                        "signalGetters": [ self._signalPositionRef ] },
                    "vel_" + name: {
                        "type": "vector",
Joseph Mirabel's avatar
Joseph Mirabel committed
87
88
                        "topic": "/hpp/target/velocity",
                        "signalGetters": [ self._signalVelocityRef ] },
Joseph Mirabel's avatar
Joseph Mirabel committed
89
90
91
                }

    def _signalPositionRef (self): return self.tp.featureDes.errorIN
92
    def _signalVelocityRef (self): return self.tp.featureDes.errordotIN
Joseph Mirabel's avatar
Joseph Mirabel committed
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120

class OpFrame(object):
    def __init__ (self, hppclient):
        self.hpp = hppclient

    def setHppGripper (self, name):
        self.name = name
        # Get parent joint and position from HPP
        self.hppjoint, self.hpppose = self.hpp.manipulation.robot.getGripperPositionInJoint(name)
        self.hpppose = Transform (self.hpppose)

    def setHppHandle (self, name):
        self.name = name
        # Get parent joint and position from HPP
        self.hppjoint, self.hpppose = self.hpp.manipulation.robot.getHandlePositionInJoint(name)
        self.hpppose = Transform (self.hpppose)

    def setSotFrameFromHpp(self, pinrobot):
        # The joint should be available in the robot model used by SOT
        n = self.hppjoint
        self.sotpose = self.hpppose
        self.robotname, self.sotjoint = parseHppName (n)
        while self.sotjoint not in pinrobot.names:
            self.sotpose = Transform(self.hpp.basic.robot.getJointPositionInParentFrame(n)) * self.sotpose
            n = self.hpp.basic.robot.getParentJointName(n)
            robotname, self.sotjoint = parseHppName (n)

class Grasp (Manifold):
Alexis Nicolin's avatar
Alexis Nicolin committed
121
    def __init__ (self, gripper, handle, otherGraspOnObject = None, closeGripper = False):
Joseph Mirabel's avatar
Joseph Mirabel committed
122
123
124
        super(Grasp, self).__init__()
        self.gripper = gripper
        self.handle = handle
Alexis Nicolin's avatar
Alexis Nicolin committed
125
126
        self.closeGripper = closeGripper

127
128
        self.relative = otherGraspOnObject is not None \
                and otherGraspOnObject.handle.hppjoint == self.handle.hppjoint
Joseph Mirabel's avatar
Joseph Mirabel committed
129
        if self.relative:
130
131
            self.otherGripper = otherGraspOnObject.gripper
            self.otherHandle = otherGraspOnObject.handle
Joseph Mirabel's avatar
Joseph Mirabel committed
132
133
134

    def makeTasks(self, sotrobot):
        from dynamic_graph.sot.core.matrix_util import matrixToTuple
Alexis Nicolin's avatar
Alexis Nicolin committed
135
136
        from dynamic_graph.sot.core import Multiply_of_matrix, Multiply_of_matrixHomo, OpPointModifier
        from dynamic_graph import plug
Joseph Mirabel's avatar
Joseph Mirabel committed
137
138
        if self.relative:
            self.graspTask = MetaTaskKine6d (
139
                    Grasp.sep + self.gripper.name + Grasp.sep + self.otherGripper.name,
Joseph Mirabel's avatar
Joseph Mirabel committed
140
141
142
                    sotrobot.dynamic,
                    self.gripper.sotjoint,
                    self.gripper.sotjoint)
Alexis Nicolin's avatar
Alexis Nicolin committed
143
144
145
146
            
            # Creates the matrix transforming the goal gripper's position into the desired moving gripper's position:
            #     on the other handle of the object
            #M = ((-1.0, 0.0, 0.0, 0.0), (0.0, 1.0, 0.0, 0.0), (0.0, 0.0, -1.0, -0.55), (0.0, 0.0, 0.0, 1.0))
147
            M = transformToMatrix(self.otherGripper.sotpose * self.otherHandle.sotpose.inverse() * self.handle.sotpose * self.gripper.sotpose.inverse())
Alexis Nicolin's avatar
Alexis Nicolin committed
148
149
            self.graspTask.opmodif = matrixToTuple(M)
            self.graspTask.feature.frame("current")
150
            setGain(self.graspTask.gain,(4.9,0.9,0.01,0.9))
Alexis Nicolin's avatar
Alexis Nicolin committed
151
152
153
            self.graspTask.task.setWithDerivative (False)

            # Sets the position and velocity of the other gripper as the goal of the first gripper pose
154
            if not self.opPointExist(sotrobot.dynamic, self.otherGripper.sotjoint):
Alexis Nicolin's avatar
Alexis Nicolin committed
155
                sotrobot.dynamic.createOpPoint(self.otherGripper.sotjoint, self.otherGripper.sotjoint)
156
                sotrobot.dynamic.createVelocity("vel_"+self.otherGripper.sotjoint, self.otherGripper.sotjoint)
Alexis Nicolin's avatar
Alexis Nicolin committed
157
158
            plug(sotrobot.dynamic.signal(self.otherGripper.sotjoint), self.graspTask.featureDes.position)
            plug(sotrobot.dynamic.signal("vel_"+self.otherGripper.sotjoint), self.graspTask.featureDes.velocity)
159
            #plug(sotrobot.dynamic.signal("J"+self.otherGripper.sotjoint), self.graspTask.featureDes.Jq)
Alexis Nicolin's avatar
Alexis Nicolin committed
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
            
            # We will need a less barbaric method to do this...
            # Zeroes the jacobian for the joints we don't want to use.
            
            #  - Create the selection matrix for the desired joints
            mat = ()
            for i in range(38):
                mat += ((0,)*i + (1 if i in range(29, 36) else 0,) + (0,)*(37-i),)
            
            # - Multiplies it with the computed jacobian matrix of the gripper
            mult = Multiply_of_matrix('mult')
            mult.sin2.value = mat
            plug(self.graspTask.opPointModif.jacobian, mult.sin1)
            plug(mult.sout, self.graspTask.feature.Jq)

            self.tasks = [ self.graspTask.task ]
            #self.tasks = []

    def opPointExist(self,dyn,opPoint):
        sigsP = filter(lambda x: x.getName().split(':')[-1] == opPoint,
                       dyn.signals())
        sigsJ = filter(lambda x: x.getName().split(':')[-1] == 'J'+opPoint,
                       dyn.signals())
        return len(sigsP)==1 & len(sigsJ)==1

Joseph Mirabel's avatar
Joseph Mirabel committed
185
186
187
188
189
190
191
192
193
class EEPosture (Manifold):
    def __init__ (self, sotrobot, gripper, position):
        from dynamic_graph.sot.core import Task, FeatureGeneric, GainAdaptive, Selec_of_vector
        from dynamic_graph.sot.core.matrix_util import matrixToTuple
        from dynamic_graph import plug
        from numpy import identity, hstack, zeros

        super(EEPosture, self).__init__()
        self.gripper = gripper
194
195
        robotName, gripperName = parseHppName (gripper.name)
        self.jointNames = sotrobot.grippers[gripperName]
Joseph Mirabel's avatar
Joseph Mirabel committed
196
197
        pinmodel = sotrobot.dynamic.model

Joseph Mirabel's avatar
Joseph Mirabel committed
198
        n = "eeposture" + Posture.sep + gripper.name + Posture.sep + str(list(position))
Joseph Mirabel's avatar
Joseph Mirabel committed
199
200
201
202
203
204
205

        self.tp = Task ('task' + n)
        self.tp.dyn = sotrobot.dynamic
        self.tp.feature = FeaturePosture ('feature_' + n)

        plug(sotrobot.dynamic.position, self.tp.feature.state)
        q = list(sotrobot.dynamic.position.value)
206
207
208
209
210
211
212
213
214
215
216
217
218
219
        # Define the reference and the selected DoF
        rank = 0
        ranks = []
        for name in self.jointNames:
            idJ = pinmodel.getJointId(name)
            assert idJ < pinmodel.njoints
            joint = pinmodel.joints[idJ]
            idx_v = joint.idx_v
            nv = joint.nv
            ranks += range(idx_v, idx_v + nv)
            q[idx_v:idx_v+nv] = position[rank: rank + nv]
            rank += nv
        assert rank == len(position)

Joseph Mirabel's avatar
Joseph Mirabel committed
220
        self.tp.feature.posture.value = q
221
222
        for i in ranks:
            self.tp.feature.selectDof (i, True)
Joseph Mirabel's avatar
Joseph Mirabel committed
223
224
225
226
227
228
229
230

        self.tp.gain = GainAdaptive("gain_"+n)
        self.tp.add(self.tp.feature.name)

        # Set the gain of the posture task
        setGain(self.tp.gain,(4.9,0.9,0.01,0.9))
        plug(self.tp.gain.gain, self.tp.controlGain)
        plug(self.tp.error, self.tp.gain.error)
231
232
        if len(self.jointNames) > 0:
            self.tasks = [ self.tp ]
Joseph Mirabel's avatar
Joseph Mirabel committed
233

Joseph Mirabel's avatar
Joseph Mirabel committed
234
class Foot (Manifold):
Alexis Nicolin's avatar
Alexis Nicolin committed
235
    def __init__ (self, footname, sotrobot, selec='111111'):
Joseph Mirabel's avatar
Joseph Mirabel committed
236
237
238
239
        robotname, sotjoint = parseHppName (footname)
        self.taskFoot = MetaTaskKine6d(
                Foot.sep + footname,
                sotrobot.dynamic,sotjoint,sotjoint)
Alexis Nicolin's avatar
Alexis Nicolin committed
240
        self.taskFoot.feature.selec.value = selec
Joseph Mirabel's avatar
Joseph Mirabel committed
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
        super(Foot, self).__init__(
                tasks = [ self.taskFoot.task, ],
                topics = {
                    footname: {
                        "velocity": False,
                        "type": "matrixHomo",
                        "handler": "hppjoint",
                        "hppjoint": footname,
                        "signalGetters": [ self._signalPositionRef ] },
                    # "vel_" + self.gripper.name: {
                    "vel_" + footname: {
                        "velocity": True,
                        "type": "vector",
                        "handler": "hppjoint",
                        "hppjoint": footname,
                        "signalGetters": [ self._signalVelocityRef ] },
                    })
Joseph Mirabel's avatar
Joseph Mirabel committed
258
        self.taskFoot.gain.value = 5
Joseph Mirabel's avatar
Joseph Mirabel committed
259
260
261
262
263

    def _signalPositionRef (self): return self.taskFoot.featureDes.position
    def _signalVelocityRef (self): return self.taskFoot.featureDes.velocity

class COM (Manifold):
264
    sep = "_com_"
Joseph Mirabel's avatar
Joseph Mirabel committed
265
266
267
    def __init__ (self, comname, sotrobot):
        self.taskCom = MetaTaskKineCom (sotrobot.dynamic,
                name = COM.sep + comname)
Joseph Mirabel's avatar
Joseph Mirabel committed
268
        self.taskCom.task.setWithDerivative (True)
Joseph Mirabel's avatar
Joseph Mirabel committed
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
        super(COM, self).__init__(
                tasks = [ self.taskCom.task, ],
                topics = {
                    comname: {
                        "velocity": False,
                        "type": "vector3",
                        "handler": "hppcom",
                        "hppcom": comname,
                        "signalGetters": [ self._signalPositionRef ] },
                    "vel_" + comname: {
                        "velocity": True,
                        "type": "vector3",
                        "handler": "hppcom",
                        "hppcom": comname,
                        "signalGetters": [ self._signalVelocityRef ] },
                    })
        self.taskCom.task.controlGain.value = 5

    def _signalPositionRef (self): return self.taskCom.featureDes.errorIN
    def _signalVelocityRef (self): return self.taskCom.featureDes.errordotIN