tools.py 12.3 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
9
10
def idx(l): return range(len(l))
def idx_zip (l): return zip (idx(l), l)

Joseph Mirabel's avatar
Joseph Mirabel committed
11
def parseHppName (hppjointname):
12
    if hppjointname == "universe": return "", "universe"
Joseph Mirabel's avatar
Joseph Mirabel committed
13
14
15
16
17
18
19
20
21
22
    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
23
24
    sep = "___"

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

    def __add__ (self, other):
Joseph Mirabel's avatar
Joseph Mirabel committed
31
        res = Manifold(list(self.tasks), list(self.constraints), dict(self.topics))
Joseph Mirabel's avatar
Joseph Mirabel committed
32
33
34
35
36
37
38
39
40
41
42
43
        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
44
45
                a["signalGetters"] += list(v["signalGetters"])
                # print k, "has", len(a["signalGetters"]), "signals"
Joseph Mirabel's avatar
Joseph Mirabel committed
46
            else:
Joseph Mirabel's avatar
Joseph Mirabel committed
47
                self.topics[k] = dict(v)
Joseph Mirabel's avatar
Joseph Mirabel committed
48
49
50
51
52
53
54
55
56
        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
57
58
59
60
61
62
63
64
65
66
67
68
        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()
69
        self.tp.feature.jacobianIN.value = matrixToTuple( identity(robotDim) )
Joseph Mirabel's avatar
Joseph Mirabel committed
70
71
72
73
        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
74
        plug(sotrobot.dynamic.position, self.tp.feature.errorIN)
Joseph Mirabel's avatar
Joseph Mirabel committed
75

76
77
        self.tp.setWithDerivative (True)

Joseph Mirabel's avatar
Joseph Mirabel committed
78
        # Set the gain of the posture task
Joseph Mirabel's avatar
Joseph Mirabel committed
79
        setGain(self.tp.gain,10)
Joseph Mirabel's avatar
Joseph Mirabel committed
80
81
82
        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
83
84
85
        self.topics = {
                    name: {
                        "type": "vector",
Joseph Mirabel's avatar
Joseph Mirabel committed
86
                        "topic": "/hpp/target/position",
Joseph Mirabel's avatar
Joseph Mirabel committed
87
88
89
                        "signalGetters": [ self._signalPositionRef ] },
                    "vel_" + name: {
                        "type": "vector",
Joseph Mirabel's avatar
Joseph Mirabel committed
90
91
                        "topic": "/hpp/target/velocity",
                        "signalGetters": [ self._signalVelocityRef ] },
Joseph Mirabel's avatar
Joseph Mirabel committed
92
93
94
                }

    def _signalPositionRef (self): return self.tp.featureDes.errorIN
95
    def _signalVelocityRef (self): return self.tp.featureDes.errordotIN
Joseph Mirabel's avatar
Joseph Mirabel committed
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
121
122
123

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
124
    def __init__ (self, gripper, handle, otherGraspOnObject = None, closeGripper = False):
Joseph Mirabel's avatar
Joseph Mirabel committed
125
126
127
        super(Grasp, self).__init__()
        self.gripper = gripper
        self.handle = handle
Alexis Nicolin's avatar
Alexis Nicolin committed
128
129
        self.closeGripper = closeGripper

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

    def makeTasks(self, sotrobot):
        from dynamic_graph.sot.core.matrix_util import matrixToTuple
Alexis Nicolin's avatar
Alexis Nicolin committed
138
139
        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
140
141
        if self.relative:
            self.graspTask = MetaTaskKine6d (
142
                    Grasp.sep + self.gripper.name + Grasp.sep + self.otherGripper.name,
Joseph Mirabel's avatar
Joseph Mirabel committed
143
144
145
                    sotrobot.dynamic,
                    self.gripper.sotjoint,
                    self.gripper.sotjoint)
Alexis Nicolin's avatar
Alexis Nicolin committed
146
147
148
149
            
            # 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))
150
            M = transformToMatrix(self.otherGripper.sotpose * self.otherHandle.sotpose.inverse() * self.handle.sotpose * self.gripper.sotpose.inverse())
Alexis Nicolin's avatar
Alexis Nicolin committed
151
152
            self.graspTask.opmodif = matrixToTuple(M)
            self.graspTask.feature.frame("current")
153
            setGain(self.graspTask.gain,(100,0.9,0.01,0.9))
Alexis Nicolin's avatar
Alexis Nicolin committed
154
155
156
            self.graspTask.task.setWithDerivative (False)

            # Sets the position and velocity of the other gripper as the goal of the first gripper pose
157
            if not self.opPointExist(sotrobot.dynamic, self.otherGripper.sotjoint):
Alexis Nicolin's avatar
Alexis Nicolin committed
158
                sotrobot.dynamic.createOpPoint(self.otherGripper.sotjoint, self.otherGripper.sotjoint)
159
                sotrobot.dynamic.createVelocity("vel_"+self.otherGripper.sotjoint, self.otherGripper.sotjoint)
Alexis Nicolin's avatar
Alexis Nicolin committed
160
161
            plug(sotrobot.dynamic.signal(self.otherGripper.sotjoint), self.graspTask.featureDes.position)
            plug(sotrobot.dynamic.signal("vel_"+self.otherGripper.sotjoint), self.graspTask.featureDes.velocity)
162
            #plug(sotrobot.dynamic.signal("J"+self.otherGripper.sotjoint), self.graspTask.featureDes.Jq)
Alexis Nicolin's avatar
Alexis Nicolin committed
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
            
            # 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
188
189
190
191
192
193
194
195
196
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
197
198
        robotName, gripperName = parseHppName (gripper.name)
        self.jointNames = sotrobot.grippers[gripperName]
Joseph Mirabel's avatar
Joseph Mirabel committed
199
200
201
202
203
204
205
206
207
208
        pinmodel = sotrobot.dynamic.model

        n = "eeposture" + Posture.sep + gripper.name + Posture.sep + str(position)

        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)
209
210
211
212
213
214
215
216
217
218
219
220
221
222
        # 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
223
        self.tp.feature.posture.value = q
224
225
        for i in ranks:
            self.tp.feature.selectDof (i, True)
Joseph Mirabel's avatar
Joseph Mirabel committed
226
227
228
229
230
231
232
233
234

        self.tp.gain = GainAdaptive("gain_"+n)
        robotDim = sotrobot.dynamic.getDimension()
        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)
235
236
        if len(self.jointNames) > 0:
            self.tasks = [ self.tp ]
Joseph Mirabel's avatar
Joseph Mirabel committed
237

Joseph Mirabel's avatar
Joseph Mirabel committed
238
class Foot (Manifold):
Alexis Nicolin's avatar
Alexis Nicolin committed
239
    def __init__ (self, footname, sotrobot, selec='111111'):
Joseph Mirabel's avatar
Joseph Mirabel committed
240
241
242
243
        robotname, sotjoint = parseHppName (footname)
        self.taskFoot = MetaTaskKine6d(
                Foot.sep + footname,
                sotrobot.dynamic,sotjoint,sotjoint)
Alexis Nicolin's avatar
Alexis Nicolin committed
244
        self.taskFoot.feature.selec.value = selec
Joseph Mirabel's avatar
Joseph Mirabel committed
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
        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
262
        self.taskFoot.gain.value = 5
Joseph Mirabel's avatar
Joseph Mirabel committed
263
264
265
266
267
268
269
270

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

class COM (Manifold):
    def __init__ (self, comname, sotrobot):
        self.taskCom = MetaTaskKineCom (sotrobot.dynamic,
                name = COM.sep + comname)
Joseph Mirabel's avatar
Joseph Mirabel committed
271
        self.taskCom.task.setWithDerivative (True)
Joseph Mirabel's avatar
Joseph Mirabel committed
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
        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