talos.py 10.9 KB
Newer Older
Pierre Fernbach's avatar
Pierre Fernbach committed
1
2
3
4
5
#!/usr/bin/env python
# Copyright (c) 2019 CNRS
# Author: Pierre Fernbach

import numpy as np
Guilhem Saurel's avatar
Guilhem Saurel committed
6
7
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody as Parent
from pinocchio import SE3
Pierre Fernbach's avatar
Pierre Fernbach committed
8

Guilhem Saurel's avatar
Guilhem Saurel committed
9
10

class Robot(Parent):
Pierre Fernbach's avatar
Pierre Fernbach committed
11
12
13
    #  Information to retrieve urdf and srdf files.
    packageName = "talos_data"
    meshPackageName = "talos_data"
Guilhem Saurel's avatar
Guilhem Saurel committed
14
    rootJointType = "freeflyer"
Pierre Fernbach's avatar
Pierre Fernbach committed
15
16
17
18
    urdfName = "talos"
    urdfSuffix = "_reduced"
    srdfSuffix = ""

19
    DEFAULT_COM_HEIGHT = 0.86
Guilhem Saurel's avatar
Guilhem Saurel committed
20
    # Information about the names of thes joints defining the limbs of the robot
Pierre Fernbach's avatar
Pierre Fernbach committed
21
22
    rLegId = 'talos_rleg_rom'
    rleg = 'leg_right_1_joint'
23
    rfoot = 'leg_right_sole_fix_joint'
Pierre Fernbach's avatar
Pierre Fernbach committed
24
25
26

    lLegId = 'talos_lleg_rom'
    lleg = 'leg_left_1_joint'
27
    lfoot = 'leg_left_sole_fix_joint'
Pierre Fernbach's avatar
Pierre Fernbach committed
28
29
30
31
32
33
34
35
36
37

    rArmId = 'talos_rarm_rom'
    rarm = 'arm_right_1_joint'
    rhand = 'arm_right_7_joint'

    lArmId = 'talos_larm_rom'
    larm = 'arm_left_1_joint'
    lhand = 'arm_left_7_joint'

    referenceConfig = [
Guilhem Saurel's avatar
Guilhem Saurel committed
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
        0.0,
        0.0,
        1.02127,
        0.0,
        0.0,
        0.0,
        1.,  # Free flyer
        0.0,
        0.0,
        -0.411354,
        0.859395,
        -0.448041,
        -0.001708,  # Left Leg
        0.0,
        0.0,
        -0.411354,
        0.859395,
        -0.448041,
        -0.001708,  # Right Leg
        0.0,
        0.006761,  # Chest
        0.25847,
        0.173046,
        -0.0002,
        -0.525366,
        0.0,
        -0.0,
        0.1,
        -0.005,  # Left Arm
        -0.25847,
        -0.173046,
        0.0002,
        -0.525366,
        0.0,
        0.0,
        0.1,
        -0.005,  # Right Arm
        0.,
        0.
    ]
Pierre Fernbach's avatar
Pierre Fernbach committed
78

79
    referenceConfig_elbowsUp = [
Guilhem Saurel's avatar
Guilhem Saurel committed
80
81
82
83
84
85
86
87
88
89
90
91
92
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
        0.0,
        0.0,
        1.02127,
        0.0,
        0.0,
        0.0,
        1.,  # Free flyer
        0.0,
        0.0,
        -0.411354,
        0.859395,
        -0.448041,
        -0.001708,  # Left Leg
        0.0,
        0.0,
        -0.411354,
        0.859395,
        -0.448041,
        -0.001708,  # Right Leg
        0.0,
        0.006761,  # Chest
        0.,
        0.173046,
        -0.0002,
        -1.5,
        0.0,
        -0.0,
        0.1,
        -0.005,  # Left Arm
        0.,
        -0.173046,
        0.0002,
        -1.5,
        0.0,
        0.0,
        0.1,
        -0.005,  # Right Arm
        0.,
        0.
    ]
Pierre Fernbach's avatar
Pierre Fernbach committed
120

Guilhem Saurel's avatar
Guilhem Saurel committed
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
    referenceConfig_legsApart = [
        0.0,
        0.0,
        1.02127,
        0.0,
        0.0,
        0.0,
        1.,  # Free flyer
        0.0,
        0.06,
        -0.411354,
        0.859395,
        -0.448041,
        -0.061708,  # Left Leg
        0.0,
        -0.06,
        -0.411354,
        0.859395,
        -0.448041,
        0.058292,  # Right Leg
        0.0,
        0.006761,  # Chest
        0.25847,
        0.173046,
        -0.0002,
        -0.525366,
        0.0,
        -0.0,
        0.1,
        -0.005,  # Left Arm
        -0.25847,
        -0.173046,
        0.0002,
        -0.525366,
        0.0,
        0.0,
        0.1,
        -0.005,  # Right Arm
        0.,
        0.
    ]
162

Guilhem Saurel's avatar
Guilhem Saurel 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
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
    referenceConfig_armsFront = [
        0.0,
        0.0,
        1.02127,
        0,
        0.0,
        0.0,
        1.0,  # Free flyer
        0.0,
        0.0,
        -0.411354,
        0.859395,
        -0.448041,
        -0.001708,  # Left Leg
        0.0,
        0.0,
        -0.411354,
        0.859395,
        -0.448041,
        -0.001708,  # Right Leg
        0.0,
        0.006761,  # Chest
        -0.5,
        0.173046,
        -0.0002,
        -0.525366,
        0.0,
        -0.0,
        0.1,
        -0.005,  # Left Arm
        0.5,
        -0.173046,
        0.0002,
        -0.525366,
        0.0,
        0.0,
        0.1,
        -0.005,  # Right Arm
        0.,
        0.
    ]

    referenceConfig_legsSide = [
        0,
        0,
        0.9832773,
        0,
        0.0,
        0.0,
        1.0,  # Free flyer
        1.57,
        0.0,
        -0.611354,
        1.059395,
        -0.448041,
        -0.001708,  # Left Leg
        -1.57,
        0.0,
        -0.611354,
        1.059395,
        -0.448041,
        -0.001708,  # Right Leg
        0.0,
        0.006761,  # Chest
        0.25847,
        0.173046,
        -0.0002,
        -0.525366,
        0.0,
        -0.0,
        0.1,
        -0.005,  # Left Arm
        -0.25847,
        -0.173046,
        0.0002,
        -0.525366,
        0.0,
        0.0,
        0.1,
        -0.005,  # Right Arm
        0.0,
        0.0
    ]

    postureWeights = [
        0,
        0,
        0,
        0,
        0,
        0,  # freeflyer
        20.,
        100.,
        0.,
        0.1,
        0.,
        1.,  # lleg
        20.,
        100.,
        0.,
        0.1,
        0.,
        1.,  # rleg
        500.,
        500.,  # chest
        50.,
        10.,
        10.,
        10.,
        10.,
        10.,
        10.,
        10.,  # larm
        50.,
        10.,
        10.,
        10.,
        10.,
        10.,
        10.,
        10.,  # rarm
        100.,
        100.
    ]

    postureWeightsRootRotationConstrained = postureWeights[::]
    postureWeightsRootRotationConstrained[3:5] = [500, 500]
Pierre Fernbach's avatar
Pierre Fernbach committed
290

Guilhem Saurel's avatar
Guilhem Saurel committed
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
    postureWeights_straff = [
        0,
        0,
        0,
        0,
        0,
        0,  # freeflyer
        100.,
        1.,
        10.,
        10,
        1.,
        0.,  # lleg
        100.,
        1.,
        10.,
        10,
        1.,
        0.,  # rleg
        500.,
        500.,  # chest
        50.,
        10.,
        10.,
        10.,
        10.,
        10.,
        10.,
        10.,  # larm
        50.,
        10.,
        10.,
        10.,
        10.,
        10.,
        10.,
        10.,  # rarm
        100.,
        100.
    ]
Pierre Fernbach's avatar
Pierre Fernbach committed
331

Guilhem Saurel's avatar
Guilhem Saurel committed
332
    # informations required to generate the limbs databases :
333
334
335
    nbSamples = 50000
    octreeSize = 0.01
    cType = "_6_DOF"
Guilhem Saurel's avatar
Guilhem Saurel committed
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
    rLegOffset = [0., 0., 0.0]
    # rLegOffset[2] += 0.005
    rLegNormal = [0, 0, 1]
    rLegx = 0.1
    rLegy = 0.06

    lLegOffset = [0., 0., 0.0]
    # lLegOffset[2] += 0.005
    lLegNormal = [0, 0, 1]
    lLegx = 0.1
    lLegy = 0.06

    rArmOffset = [-0.01, 0., -0.154]
    rArmNormal = [0, 0, 1]
    rArmx = 0.005
    rArmy = 0.005

    lArmOffset = [-0.01, 0., -0.154]
    lArmNormal = [0, 0, 1]
    lArmx = 0.005
    lArmy = 0.005

    kinematicConstraintsPath = "package://talos-rbprm/com_inequalities/"
    rLegKinematicConstraints = kinematicConstraintsPath + rleg + "_com_constraints.obj"
    lLegKinematicConstraints = kinematicConstraintsPath + lleg + "_com_constraints.obj"
    rArmKinematicConstraints = kinematicConstraintsPath + rarm + "_com_constraints.obj"
    lArmKinematicConstraints = kinematicConstraintsPath + larm + "_com_constraints.obj"
    minDist = 0.4
    # data used by scripts :
    limbs_names = [rLegId, lLegId, rArmId, lArmId]
    dict_limb_rootJoint = {rLegId: rleg, lLegId: lleg, rArmId: rarm, lArmId: larm}
    dict_limb_joint = {rLegId: rfoot, lLegId: lfoot, rArmId: rhand, lArmId: lhand}
    dict_limb_color_traj = {rfoot: [0, 1, 0, 1], lfoot: [1, 0, 0, 1], rhand: [0, 0, 1, 1], lhand: [0.9, 0.5, 0, 1]}
Pierre Fernbach's avatar
Pierre Fernbach committed
369
370
    FOOT_SAFETY_SIZE = 0.03
    # size of the contact surface (x,y)
Guilhem Saurel's avatar
Guilhem Saurel committed
371
    dict_size = {rfoot: [0.2, 0.13], lfoot: [0.2, 0.13], rhand: [0.1, 0.1], lhand: [0.1, 0.1]}
Pierre Fernbach's avatar
Pierre Fernbach committed
372

Guilhem Saurel's avatar
Guilhem Saurel committed
373
    # various offset used by scripts :
Pierre Fernbach's avatar
Pierre Fernbach committed
374
375

    MRsole_offset = SE3.Identity()
376
    MRsole_offset.translation = np.matrix(rLegOffset).T
Pierre Fernbach's avatar
Pierre Fernbach committed
377
    MLsole_offset = SE3.Identity()
378
    MLsole_offset.translation = np.matrix(lLegOffset).T
Pierre Fernbach's avatar
Pierre Fernbach committed
379
    MRhand_offset = SE3.Identity()
380
    MRhand_offset.translation = np.matrix(rArmOffset).T
Pierre Fernbach's avatar
Pierre Fernbach committed
381
    MLhand_offset = SE3.Identity()
382
    MLhand_offset.translation = np.matrix(lArmOffset).T
Guilhem Saurel's avatar
Guilhem Saurel committed
383
384
    dict_offset = {rfoot: MRsole_offset, lfoot: MLsole_offset, rhand: MRhand_offset, lhand: MLhand_offset}
    dict_normal = {rfoot: rLegNormal, lfoot: lLegNormal, rhand: rArmNormal, lhand: lArmNormal}
Pierre Fernbach's avatar
Pierre Fernbach committed
385
386
387

    # display transform :

Guilhem Saurel's avatar
Guilhem Saurel committed
388
389
    # MRsole_display = MRsole_offset.copy()
    # MLsole_display = MLsole_offset.copy()
Pierre Fernbach's avatar
Pierre Fernbach committed
390
391
392
    MRsole_display = SE3.Identity()
    MLsole_display = SE3.Identity()
    MRhand_display = SE3.Identity()
Guilhem Saurel's avatar
Guilhem Saurel committed
393
    # MRhand_display.translation = np.matrix([0,  0., -0.11])
Pierre Fernbach's avatar
Pierre Fernbach committed
394
    MLhand_display = SE3.Identity()
Guilhem Saurel's avatar
Guilhem Saurel committed
395
396
    # MLhand_display.translation = np.matrix([0,  0., -0.11])
    dict_display_offset = {rfoot: MRsole_display, lfoot: MLsole_display, rhand: MRhand_display, lhand: MLhand_display}
Pierre Fernbach's avatar
Pierre Fernbach committed
397

Guilhem Saurel's avatar
Guilhem Saurel committed
398
    kneeIds = {"Left": 10, "Right": 16}
Pierre Fernbach's avatar
Pierre Fernbach committed
399

400
401
    def __init__(self, name=None, load=True, client=None, clientRbprm=None):
        Parent.__init__(self, load, clientRbprm)
Pierre Fernbach's avatar
Pierre Fernbach committed
402
        if load:
Guilhem Saurel's avatar
Guilhem Saurel committed
403
            self.loadFullBodyModel(self.urdfName, self.rootJointType, self.meshPackageName, self.packageName,
404
                                   self.urdfSuffix, self.srdfSuffix, client)
Guilhem Saurel's avatar
Guilhem Saurel committed
405
        if name is not None:
Pierre Fernbach's avatar
Pierre Fernbach committed
406
            self.name = name
Guilhem Saurel's avatar
Guilhem Saurel committed
407
408
409
410
411
412
413
414
        self.joint1L_bounds_prev = self.getJointBounds('leg_left_1_joint')
        self.joint6L_bounds_prev = self.getJointBounds('leg_left_6_joint')
        self.joint2L_bounds_prev = self.getJointBounds('leg_left_2_joint')
        self.joint1R_bounds_prev = self.getJointBounds('leg_right_1_joint')
        self.joint6R_bounds_prev = self.getJointBounds('leg_right_6_joint')
        self.joint2R_bounds_prev = self.getJointBounds('leg_right_2_joint')

    def loadAllLimbs(self, heuristic, analysis=None, nbSamples=nbSamples, octreeSize=octreeSize):
415
416
        for id in self.limbs_names:
            eff = self.dict_limb_joint[id]
Guilhem Saurel's avatar
Guilhem Saurel committed
417
418
419
            self.addLimb(id,
                         self.dict_limb_rootJoint[id],
                         eff,
420
                         self.dict_offset[eff].translation.tolist(),
Guilhem Saurel's avatar
Guilhem Saurel committed
421
422
423
424
425
426
427
428
429
430
431
                         self.dict_normal[eff],
                         self.dict_size[eff][0] / 2.,
                         self.dict_size[eff][1] / 2.,
                         nbSamples,
                         heuristic,
                         octreeSize,
                         self.cType,
                         kinematicConstraintsPath=self.kinematicConstraintsPath + self.dict_limb_rootJoint[id] +
                         "_com_constraints.obj",
                         kinematicConstraintsMin=self.minDist)
            if analysis:
432
                self.runLimbSampleAnalysis(id, analysis, True)
433
434

    def setConstrainedJointsBounds(self):
Guilhem Saurel's avatar
Guilhem Saurel committed
435
436
437
438
439
440
        self.setJointBounds('leg_left_1_joint', [-0.34, 1.4])
        self.setJointBounds('leg_left_6_joint', [-0.25, 0.25])
        self.setJointBounds('leg_left_2_joint', [-0.25, 0.25])
        self.setJointBounds('leg_right_1_joint', [-1.4, 0.34])
        self.setJointBounds('leg_right_6_joint', [-0.25, 0.25])
        self.setJointBounds('leg_right_2_joint', [-0.25, 0.25])
441
442

    def resetJointsBounds(self):
Guilhem Saurel's avatar
Guilhem Saurel committed
443
444
445
446
447
448
        self.setJointBounds('leg_left_1_joint', self.joint1L_bounds_prev)
        self.setJointBounds('leg_left_6_joint', self.joint6L_bounds_prev)
        self.setJointBounds('leg_left_2_joint', self.joint2L_bounds_prev)
        self.setJointBounds('leg_right_1_joint', self.joint1R_bounds_prev)
        self.setJointBounds('leg_right_6_joint', self.joint6R_bounds_prev)
        self.setJointBounds('leg_right_2_joint', self.joint2R_bounds_prev)