robots_loader.py 17.2 KB
Newer Older
1
import sys
2
import warnings
3
4
from os.path import dirname, exists, join

Guilhem Saurel's avatar
Guilhem Saurel committed
5
import numpy as np
6
import pinocchio as pin
7
8
from pinocchio.robot_wrapper import RobotWrapper

9
pin.switchToNumpyArray()
10

11

Guilhem Saurel's avatar
Guilhem Saurel committed
12
13
14
15
def _depr_msg(deprecated, key):
    return "`%s` is deprecated. Please use `load('%s')`" % (deprecated, key)


16
def getModelPath(subpath, printmsg=False):
17
    source = dirname(dirname(dirname(__file__)))  # top level source directory
18
    paths = [
19
20
21
        join(dirname(dirname(dirname(source))), 'robots'),  # function called from "make release" in build/ dir
        join(dirname(source), 'robots'),  # function called from a build/ dir inside top level source
        join(source, 'robots')  # function called from top level source dir
22
23
    ]
    try:
24
25
26
        from .path import EXAMPLE_ROBOT_DATA_MODEL_DIR, EXAMPLE_ROBOT_DATA_SOURCE_DIR
        paths.append(EXAMPLE_ROBOT_DATA_MODEL_DIR)  # function called from installed project
        paths.append(EXAMPLE_ROBOT_DATA_SOURCE_DIR)  # function called from off-tree build dir
27
28
29
30
    except ImportError:
        pass
    paths += [join(p, '../../../share/example-robot-data/robots') for p in sys.path]
    for path in paths:
31
        if exists(join(path, subpath.strip('/'))):
32
33
            if printmsg:
                print("using %s as modelPath" % path)
34
            return path
35
    raise IOError('%s not found' % subpath)
36
37


38
def readParamsFromSrdf(model, SRDF_PATH, verbose=False, has_rotor_parameters=True, referencePose='half_sitting'):
39
    if has_rotor_parameters:
40
        pin.loadRotorParameters(model, SRDF_PATH, verbose)
41
    model.armature = np.multiply(model.rotorInertia.flat, np.square(model.rotorGearRatio.flat))
42
43
    pin.loadReferenceConfigurations(model, SRDF_PATH, verbose)
    q0 = pin.neutral(model)
Guilhem Saurel's avatar
Guilhem Saurel committed
44
    if referencePose is not None:
45
46
        q0 = model.referenceConfigurations[referencePose].copy()
    return q0
Guilhem Saurel's avatar
Guilhem Saurel committed
47

48

Guilhem Saurel's avatar
Guilhem Saurel committed
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
class RobotLoader(object):
    path = ''
    urdf_filename = ''
    srdf_filename = ''
    urdf_subpath = 'robots'
    srdf_subpath = 'srdf'
    ref_posture = 'half_sitting'
    has_rotor_parameters = False
    free_flyer = False
    verbose = False

    def __init__(self):
        urdf_path = join(self.path, self.urdf_subpath, self.urdf_filename)
        self.model_path = getModelPath(urdf_path, self.verbose)
        self.urdf_path = join(self.model_path, urdf_path)
        self.robot = RobotWrapper.BuildFromURDF(self.urdf_path, [join(self.model_path, '../..')],
                                                pin.JointModelFreeFlyer() if self.free_flyer else None)

        if self.srdf_filename:
            self.srdf_path = join(self.model_path, self.path, self.srdf_subpath, self.srdf_filename)
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
69
70
            self.robot.q0 = readParamsFromSrdf(self.robot.model, self.srdf_path, self.verbose,
                                               self.has_rotor_parameters, self.ref_posture)
71
72
73
74
75
76
77
78
79
80

            if pin.WITH_HPP_FCL and pin.WITH_HPP_FCL_BINDINGS:
                # Add all collision pairs
                self.robot.collision_model.addAllCollisionPairs()

                # Remove collision pairs per SRDF
                pin.removeCollisionPairs(self.robot.model, self.robot.collision_model, self.srdf_path, False)

                # Recreate collision data since the collision pairs changed
                self.robot.collision_data = self.robot.collision_model.createData()
Guilhem Saurel's avatar
Guilhem Saurel committed
81
82
        else:
            self.srdf_path = None
83
            self.robot.q0 = pin.neutral(self.robot.model)
Guilhem Saurel's avatar
Guilhem Saurel committed
84
85
86
87
88
89
90
91
92
93
94
95

        if self.free_flyer:
            self.addFreeFlyerJointLimits()

    def addFreeFlyerJointLimits(self):
        ub = self.robot.model.upperPositionLimit
        ub[:7] = 1
        self.robot.model.upperPositionLimit = ub
        lb = self.robot.model.lowerPositionLimit
        lb[:7] = -1
        self.robot.model.lowerPositionLimit = lb

96
97
98
99
100
    @property
    def q0(self):
        warnings.warn("`q0` is deprecated. Please use `robot.q0`", FutureWarning, 2)
        return self.robot.q0

Guilhem Saurel's avatar
Guilhem Saurel committed
101

102
103
class A1Loader(RobotLoader):
    path = 'a1_description'
104
    urdf_filename = "a1.urdf"
105
106
107
108
109
110
    urdf_subpath = "urdf"
    srdf_filename = "a1.srdf"
    ref_posture = "standing"
    free_flyer = True


Guilhem Saurel's avatar
Guilhem Saurel committed
111
112
113
114
115
116
117
118
119
120
121
122
class ANYmalLoader(RobotLoader):
    path = 'anymal_b_simple_description'
    urdf_filename = "anymal.urdf"
    srdf_filename = "anymal.srdf"
    ref_posture = "standing"
    free_flyer = True


class ANYmalKinovaLoader(ANYmalLoader):
    urdf_filename = "anymal-kinova.urdf"
    srdf_filename = "anymal-kinova.srdf"
    ref_posture = "standing_with_arm_up"
123
124


Guilhem Saurel's avatar
Guilhem Saurel committed
125
126
127
128
129
130
class BaxterLoader(RobotLoader):
    path = "baxter_description"
    urdf_filename = "baxter.urdf"
    urdf_subpath = "urdf"


Guilhem Saurel's avatar
Guilhem Saurel committed
131
132
def loadANYmal(withArm=None):
    if withArm:
Guilhem Saurel's avatar
Guilhem Saurel committed
133
        warnings.warn(_depr_msg('loadANYmal(kinova)', 'anymal_kinova'), FutureWarning, 2)
Guilhem Saurel's avatar
Guilhem Saurel committed
134
135
        loader = ANYmalKinovaLoader
    else:
Guilhem Saurel's avatar
Guilhem Saurel committed
136
        warnings.warn(_depr_msg('loadANYmal()', 'anymal'), FutureWarning, 2)
Guilhem Saurel's avatar
Guilhem Saurel committed
137
138
        loader = ANYmalLoader
    return loader().robot
139
140


Guilhem Saurel's avatar
Guilhem Saurel committed
141
142
143
144
145
class TalosLoader(RobotLoader):
    path = 'talos_data'
    urdf_filename = "talos_reduced.urdf"
    srdf_filename = "talos.srdf"
    free_flyer = True
146
147


Guilhem Saurel's avatar
Guilhem Saurel committed
148
149
class TalosBoxLoader(TalosLoader):
    urdf_filename = "talos_reduced_box.urdf"
150
151


Guilhem Saurel's avatar
Guilhem Saurel committed
152
153
class TalosFullLoader(TalosLoader):
    urdf_filename = "talos_full_v2.urdf"
154
155


Guilhem Saurel's avatar
Guilhem Saurel committed
156
157
class TalosFullBoxLoader(TalosLoader):
    urdf_filename = "talos_full_v2_box.urdf"
158

Guilhem Saurel's avatar
Guilhem Saurel committed
159
160
161
162
163
164
165
166
167

class TalosArmLoader(TalosLoader):
    urdf_filename = "talos_left_arm.urdf"
    free_flyer = False


class TalosLegsLoader(TalosLoader):
    def __init__(self):
        super(TalosLegsLoader, self).__init__()
168
        legMaxId = 14
Guilhem Saurel's avatar
Guilhem Saurel committed
169
        m1 = self.robot.model
170
171
172
173
        m2 = pin.Model()
        for j, M, name, parent, Y in zip(m1.joints, m1.jointPlacements, m1.names, m1.parents, m1.inertias):
            if j.id < legMaxId:
                jid = m2.addJoint(parent, getattr(pin, j.shortname())(), M, name)
174
175
176
177
178
                idx_q, idx_v = m2.joints[jid].idx_q, m2.joints[jid].idx_v
                m2.upperPositionLimit[idx_q:idx_q + j.nq] = m1.upperPositionLimit[j.idx_q:j.idx_q + j.nq]
                m2.lowerPositionLimit[idx_q:idx_q + j.nq] = m1.lowerPositionLimit[j.idx_q:j.idx_q + j.nq]
                m2.velocityLimit[idx_v:idx_v + j.nv] = m1.velocityLimit[j.idx_v:j.idx_v + j.nv]
                m2.effortLimit[idx_v:idx_v + j.nv] = m1.effortLimit[j.idx_v:j.idx_v + j.nv]
179
                assert jid == j.id
180
181
182
183
184
185
186
187
188
189
190
191
                m2.appendBodyToJoint(jid, Y, pin.SE3.Identity())

        upperPos = m2.upperPositionLimit
        upperPos[:7] = 1
        m2.upperPositionLimit = upperPos
        lowerPos = m2.lowerPositionLimit
        lowerPos[:7] = -1
        m2.lowerPositionLimit = lowerPos
        effort = m2.effortLimit
        effort[:6] = np.inf
        m2.effortLimit = effort

Guilhem Saurel's avatar
Guilhem Saurel committed
192
        # q2 = self.robot.q0[:19]
193
194
195
196
197
        for f in m1.frames:
            if f.parent < legMaxId:
                m2.addFrame(f)

        g2 = pin.GeometryModel()
Guilhem Saurel's avatar
Guilhem Saurel committed
198
        for g in self.robot.visual_model.geometryObjects:
199
200
201
            if g.parentJoint < 14:
                g2.addGeometryObject(g)

Guilhem Saurel's avatar
Guilhem Saurel committed
202
203
204
205
206
        self.robot.model = m2
        self.robot.data = m2.createData()
        self.robot.visual_model = g2
        # self.robot.q0=q2
        self.robot.visual_data = pin.GeometryData(g2)
207
208

        # Load SRDF file
Guilhem Saurel's avatar
format    
Guilhem Saurel committed
209
210
        self.robot.q0 = readParamsFromSrdf(self.robot.model, self.srdf_path, self.verbose, self.has_rotor_parameters,
                                           self.ref_posture)
211

212
        assert (m2.armature[:6] == 0.).all()
213
        # Add the free-flyer joint limits to the new model
Guilhem Saurel's avatar
Guilhem Saurel committed
214
        self.addFreeFlyerJointLimits()
215

Guilhem Saurel's avatar
Guilhem Saurel committed
216
217
218

def loadTalos(legs=False, arm=False, full=False, box=False):
    if legs:
219
        warnings.warn(_depr_msg('loadTalos(legs)', 'talos_legs'), FutureWarning, 2)
Guilhem Saurel's avatar
Guilhem Saurel committed
220
221
        loader = TalosLegsLoader
    elif arm:
222
        warnings.warn(_depr_msg('loadTalos(arm)', 'talos_arm'), FutureWarning, 2)
Guilhem Saurel's avatar
Guilhem Saurel committed
223
224
225
        loader = TalosArmLoader
    elif full:
        if box:
226
            warnings.warn(_depr_msg('loadTalos(full, box)', 'talos_full_box'), FutureWarning, 2)
Guilhem Saurel's avatar
Guilhem Saurel committed
227
228
            loader = TalosFullBoxLoader
        else:
229
            warnings.warn(_depr_msg('loadTalos(full)', 'talos_full'), FutureWarning, 2)
Guilhem Saurel's avatar
Guilhem Saurel committed
230
231
232
            loader = TalosFullLoader
    else:
        if box:
233
            warnings.warn(_depr_msg('loadTalos(box)', 'talos_box'), FutureWarning, 2)
Guilhem Saurel's avatar
Guilhem Saurel committed
234
235
            loader = TalosBoxLoader
        else:
236
            warnings.warn(_depr_msg('loadTalos()', 'talos'), FutureWarning, 2)
Guilhem Saurel's avatar
Guilhem Saurel committed
237
238
            loader = TalosLoader
    return loader().robot
239

Guilhem Saurel's avatar
Guilhem Saurel committed
240

Carlos Mastalli's avatar
Carlos Mastalli committed
241
def loadTalosLegs():
242
    warnings.warn(_depr_msg('loadTalosLegs()', 'talos_legs'), FutureWarning, 2)
243
    return loadTalos(legs=True)
Carlos Mastalli's avatar
Carlos Mastalli committed
244

245

246
def loadTalosArm():
247
    warnings.warn(_depr_msg('loadTalosArm()', 'talos_arm'), FutureWarning, 2)
248
249
250
    return loadTalos(arm=True)


Guilhem Saurel's avatar
Guilhem Saurel committed
251
252
253
254
255
256
257
258
class HyQLoader(RobotLoader):
    path = "hyq_description"
    urdf_filename = "hyq_no_sensors.urdf"
    srdf_filename = "hyq.srdf"
    ref_posture = "standing"
    free_flyer = True


259
def loadHyQ():
260
    warnings.warn(_depr_msg('loadHyQ()', 'hyq'), FutureWarning, 2)
Guilhem Saurel's avatar
Guilhem Saurel committed
261
262
263
    return HyQLoader().robot


Julian Viereck's avatar
Merge    
Julian Viereck committed
264
265
266
267
268
269
270
271
class BoltLoader(RobotLoader):
    path = 'bolt_description'
    urdf_filename = "bolt.urdf"
    srdf_filename = "bolt.srdf"
    ref_posture = "standing"
    free_flyer = True


272
class Solo8Loader(RobotLoader):
Guilhem Saurel's avatar
Guilhem Saurel committed
273
274
275
276
277
278
279
    path = 'solo_description'
    urdf_filename = "solo.urdf"
    srdf_filename = "solo.srdf"
    ref_posture = "standing"
    free_flyer = True


Julian Viereck's avatar
Julian Viereck committed
280
281
282
283
284
285
class SoloLoader(Solo8Loader):
    def __init__(self, *args, **kwargs):
        warnings.warn('"solo" is deprecated, please try to load "solo8"')
        return super(SoloLoader, self).__init__(*args, **kwargs)


286
class Solo12Loader(Solo8Loader):
Guilhem Saurel's avatar
Guilhem Saurel committed
287
    urdf_filename = "solo12.urdf"
288
289


Julian Viereck's avatar
Julian Viereck committed
290
def loadSolo(solo=True):
291
    warnings.warn(_depr_msg('loadSolo()', 'solo8'), FutureWarning, 2)
Julian Viereck's avatar
Julian Viereck committed
292
    loader = Solo8Loader if solo else Solo12Loader
Guilhem Saurel's avatar
Guilhem Saurel committed
293
294
    return loader().robot

295

Julian Viereck's avatar
Julian Viereck committed
296
297
298
299
300
301
302
class FingerEduLoader(RobotLoader):
    path = 'finger_edu_description'
    urdf_filename = "finger_edu.urdf"
    srdf_filename = "finger_edu.srdf"
    ref_posture = "hanging"
    free_flyer = False

Guilhem Saurel's avatar
Guilhem Saurel committed
303
304
305
306
307
308

class KinovaLoader(RobotLoader):
    path = "kinova_description"
    urdf_filename = "kinova.urdf"
    srdf_filename = "kinova.srdf"
    ref_posture = "arm_up"
Carlos Mastalli's avatar
Carlos Mastalli committed
309
310


311
def loadKinova():
312
    warnings.warn(_depr_msg('loadKinova()', 'kinova'), FutureWarning, 2)
Guilhem Saurel's avatar
Guilhem Saurel committed
313
314
315
316
317
318
319
320
    return KinovaLoader().robot


class TiagoLoader(RobotLoader):
    path = "tiago_description"
    urdf_filename = "tiago.urdf"


321
322
323
324
class TiagoDualLoader(TiagoLoader):
    urdf_filename = "tiago_dual.urdf"


Guilhem Saurel's avatar
Guilhem Saurel committed
325
326
class TiagoNoHandLoader(TiagoLoader):
    urdf_filename = "tiago_no_hand.urdf"
327

328

Guilhem Saurel's avatar
Guilhem Saurel committed
329
def loadTiago(hand=True):
Guilhem Saurel's avatar
Guilhem Saurel committed
330
    if hand:
331
        warnings.warn(_depr_msg('loadTiago()', 'tiago'), FutureWarning, 2)
Guilhem Saurel's avatar
Guilhem Saurel committed
332
333
        loader = TiagoLoader
    else:
334
        warnings.warn(_depr_msg('loadTiago(hand=False)', 'tiago_no_hand'), FutureWarning, 2)
Guilhem Saurel's avatar
Guilhem Saurel committed
335
336
        loader = TiagoNoHandLoader
    return loader().robot
337
338
339


def loadTiagoNoHand():
340
    warnings.warn(_depr_msg('loadTiagoNoHand()', 'tiago_no_hand'), FutureWarning, 2)
Guilhem Saurel's avatar
Guilhem Saurel committed
341
    return loadTiago(hand=False)
342

Carlos Mastalli's avatar
Carlos Mastalli committed
343

Guilhem Saurel's avatar
Guilhem Saurel committed
344
345
346
347
348
349
350
351
352
353
354
class ICubLoader(RobotLoader):
    path = "icub_description"
    urdf_filename = "icub.urdf"
    srdf_filename = "icub.srdf"
    free_flyer = True


class ICubReducedLoader(ICubLoader):
    urdf_filename = "icub_reduced.urdf"


355
def loadICub(reduced=True):
Guilhem Saurel's avatar
Guilhem Saurel committed
356
    if reduced:
357
        warnings.warn(_depr_msg('loadICub()', 'icub_reduced'), FutureWarning, 2)
Guilhem Saurel's avatar
Guilhem Saurel committed
358
359
        loader = ICubReducedLoader
    else:
360
        warnings.warn(_depr_msg('loadICub(reduced=False)', 'icub'), FutureWarning, 2)
Guilhem Saurel's avatar
Guilhem Saurel committed
361
362
363
364
365
366
367
368
        loader = ICubLoader
    return loader().robot


class PandaLoader(RobotLoader):
    path = "panda_description"
    urdf_filename = "panda.urdf"
    urdf_subpath = "urdf"
Guilhem Saurel's avatar
Guilhem Saurel committed
369
370


Justin Carpentier's avatar
Justin Carpentier committed
371
def loadPanda():
372
    warnings.warn(_depr_msg('loadPanda()', 'panda'), FutureWarning, 2)
Guilhem Saurel's avatar
Guilhem Saurel committed
373
    return PandaLoader().robot
Justin Carpentier's avatar
Justin Carpentier committed
374

375

Guilhem Saurel's avatar
Guilhem Saurel committed
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
class UR3Loader(RobotLoader):
    path = "ur_description"
    urdf_filename = "ur3_robot.urdf"
    urdf_subpath = "urdf"
    ref_posture = None


class UR3GripperLoader(UR3Loader):
    urdf_filename = "ur3_gripper.urdf"
    srdf_filename = "ur3_gripper.srdf"


class UR3LimitedLoader(UR3Loader):
    urdf_filename = "ur3_joint_limited_robot.urdf"


class UR5Loader(UR3Loader):
    urdf_filename = "ur5_robot.urdf"
    srdf_filename = "ur5.srdf"


class UR5GripperLoader(UR5Loader):
    urdf_filename = "ur5_gripper.urdf"
    srdf_filename = "ur5_gripper.srdf"


class UR5LimitedLoader(UR5Loader):
    urdf_filename = "ur5_joint_limited_robot.urdf"

405

Guilhem Saurel's avatar
Guilhem Saurel committed
406
407
408
409
410
411
412
413
414
415
416
class UR10Loader(UR3Loader):
    urdf_filename = "ur10_robot.urdf"


class UR10LimitedLoader(UR10Loader):
    urdf_filename = "ur10_joint_limited_robot.urdf"


def loadUR(robot=5, limited=False, gripper=False):
    if robot == 3:
        if limited:
417
            warnings.warn(_depr_msg('loadUr(3, limited)', 'ur3_limited'), FutureWarning, 2)
Guilhem Saurel's avatar
Guilhem Saurel committed
418
419
            loader = UR3LimitedLoader
        elif gripper:
420
            warnings.warn(_depr_msg('loadUr(3, gripper)', 'ur3_gripper'), FutureWarning, 2)
Guilhem Saurel's avatar
Guilhem Saurel committed
421
422
            loader = UR3GripperLoader
        else:
423
            warnings.warn(_depr_msg('loadUr(3)', 'ur3'), FutureWarning, 2)
Guilhem Saurel's avatar
Guilhem Saurel committed
424
425
426
            loader = UR3Loader
    elif robot == 5:
        if limited:
427
            warnings.warn(_depr_msg('loadUr(limited)', 'ur5_limited'), FutureWarning, 2)
Guilhem Saurel's avatar
Guilhem Saurel committed
428
429
            loader = UR5LimitedLoader
        elif gripper:
430
            warnings.warn(_depr_msg('loadUr(gripper)', 'ur5_gripper'), FutureWarning, 2)
Guilhem Saurel's avatar
Guilhem Saurel committed
431
432
            loader = UR5GripperLoader
        else:
433
            warnings.warn(_depr_msg('loadUr()', 'ur5'), FutureWarning, 2)
Guilhem Saurel's avatar
Guilhem Saurel committed
434
435
436
            loader = UR5Loader
    elif robot == 10:
        if limited:
437
            warnings.warn(_depr_msg('loadUr(10, limited)', 'ur10_limited'), FutureWarning, 2)
Guilhem Saurel's avatar
Guilhem Saurel committed
438
439
            loader = UR10LimitedLoader
        else:
440
            warnings.warn(_depr_msg('loadUr(10)', 'ur10'), FutureWarning, 2)
Guilhem Saurel's avatar
Guilhem Saurel committed
441
442
443
444
445
446
447
448
            loader = UR10Loader
    return loader().robot


class HectorLoader(RobotLoader):
    path = "hector_description"
    urdf_filename = "quadrotor_base.urdf"
    free_flyer = True
449
450
451


def loadHector():
452
    warnings.warn(_depr_msg('loadHector()', 'hector'), FutureWarning, 2)
Guilhem Saurel's avatar
Guilhem Saurel committed
453
454
455
456
457
458
459
    return HectorLoader().robot


class DoublePendulumLoader(RobotLoader):
    path = "double_pendulum_description"
    urdf_filename = "double_pendulum.urdf"
    urdf_subpath = "urdf"
460
461


Pep Marti Saumell's avatar
Pep Marti Saumell committed
462
def loadDoublePendulum():
463
    warnings.warn(_depr_msg('loadDoublePendulum()', 'double_pendulum'), FutureWarning, 2)
Guilhem Saurel's avatar
Guilhem Saurel committed
464
465
466
467
468
469
470
471
    return DoublePendulumLoader().robot


class RomeoLoader(RobotLoader):
    path = "romeo_description"
    urdf_filename = "romeo.urdf"
    urdf_subpath = "urdf"
    free_flyer = True
Pep Marti Saumell's avatar
Pep Marti Saumell committed
472
473


Guilhem Saurel's avatar
Guilhem Saurel committed
474
def loadRomeo():
475
    warnings.warn(_depr_msg('loadRomeo()', 'romeo'), FutureWarning, 2)
Guilhem Saurel's avatar
Guilhem Saurel committed
476
477
478
    return RomeoLoader().robot


479
480
481
482
483
484
485
486
487
488
489
490
491
class SimpleHumanoidLoader(RobotLoader):
    path = 'simple_humanoid_description'
    urdf_subpath = 'urdf'
    urdf_filename = 'simple_humanoid.urdf'
    srdf_filename = 'simple_humanoid.srdf'
    free_flyer = True


class SimpleHumanoidClassicalLoader(SimpleHumanoidLoader):
    urdf_filename = 'simple_humanoid_classical.urdf'
    srdf_filename = 'simple_humanoid_classical.srdf'


Guilhem Saurel's avatar
Guilhem Saurel committed
492
493
494
495
class IrisLoader(RobotLoader):
    path = "iris_description"
    urdf_filename = "iris_simple.urdf"
    free_flyer = True
496

497

498
def loadIris():
499
    warnings.warn(_depr_msg('loadIris()', 'iris'), FutureWarning, 2)
Guilhem Saurel's avatar
Guilhem Saurel committed
500
    return IrisLoader().robot
501
502
503


ROBOTS = {
504
    'a1': A1Loader,
Guilhem Saurel's avatar
Guilhem Saurel committed
505
506
    'anymal': ANYmalLoader,
    'anymal_kinova': ANYmalKinovaLoader,
Guilhem Saurel's avatar
Guilhem Saurel committed
507
    'baxter': BaxterLoader,
Guilhem Saurel's avatar
Guilhem Saurel committed
508
509
510
511
512
513
514
515
516
    'double_pendulum': DoublePendulumLoader,
    'hector': HectorLoader,
    'hyq': HyQLoader,
    'icub': ICubLoader,
    'icub_reduced': ICubReducedLoader,
    'iris': IrisLoader,
    'kinova': KinovaLoader,
    'panda': PandaLoader,
    'romeo': RomeoLoader,
517
518
    'simple_humanoid': SimpleHumanoidLoader,
    'simple_humanoid_classical': SimpleHumanoidClassicalLoader,
Julian Viereck's avatar
Merge    
Julian Viereck committed
519
    'bolt': BoltLoader,
Guilhem Saurel's avatar
Guilhem Saurel committed
520
    'solo': SoloLoader,
Julian Viereck's avatar
Julian Viereck committed
521
    'solo8': Solo8Loader,
Guilhem Saurel's avatar
Guilhem Saurel committed
522
    'solo12': Solo12Loader,
Julian Viereck's avatar
Julian Viereck committed
523
    'finger_edu': FingerEduLoader,
Guilhem Saurel's avatar
Guilhem Saurel committed
524
525
526
527
528
529
530
    'talos': TalosLoader,
    'talos_box': TalosBoxLoader,
    'talos_arm': TalosArmLoader,
    'talos_legs': TalosLegsLoader,
    'talos_full': TalosFullLoader,
    'talos_full_box': TalosFullBoxLoader,
    'tiago': TiagoLoader,
531
    'tiago_dual': TiagoDualLoader,
Guilhem Saurel's avatar
Guilhem Saurel committed
532
533
534
535
536
537
538
539
540
    'tiago_no_hand': TiagoNoHandLoader,
    'ur3': UR5Loader,
    'ur3_gripper': UR3GripperLoader,
    'ur3_limited': UR3LimitedLoader,
    'ur5': UR5Loader,
    'ur5_gripper': UR5GripperLoader,
    'ur5_limited': UR5LimitedLoader,
    'ur10': UR10Loader,
    'ur10_limited': UR10LimitedLoader,
541
542
543
}


544
def loader(name, display=False, rootNodeName=''):
545
    """Load a robot by its name, and optionnaly display it in a viewer."""
Guilhem Saurel's avatar
Guilhem Saurel committed
546
547
548
549
    if name not in ROBOTS:
        robots = ", ".join(sorted(ROBOTS.keys()))
        raise ValueError("Robot '%s' not found. Possible values are %s" % (name, robots))
    inst = ROBOTS[name]()
550
    if display:
551
552
553
554
555
556
        if rootNodeName:
            inst.robot.initViewer()
            inst.robot.viz.loadViewerModel(rootNodeName=rootNodeName)
        else:
            inst.robot.initViewer(loadModel=True)
        inst.robot.display(inst.robot.q0)
Guilhem Saurel's avatar
Guilhem Saurel committed
557
558
559
    return inst


560
def load(name, display=False, rootNodeName=''):
Guilhem Saurel's avatar
Guilhem Saurel committed
561
    """Load a robot by its name, and optionnaly display it in a viewer."""
562
    return loader(name, display, rootNodeName).robot
Guilhem Saurel's avatar
Guilhem Saurel committed
563
564


565
def load_full(name, display=False, rootNodeName=''):
Guilhem Saurel's avatar
Guilhem Saurel committed
566
    """Load a robot by its name, optionnaly display it in a viewer, and provide its q0 and paths."""
567
    inst = loader(name, display, rootNodeName)
568
    return inst.robot, inst.robot.q0, inst.urdf_path, inst.srdf_path