robots_loader.py 3.7 KB
Newer Older
1
2
from os.path import exists, join

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


8
9
10
def getModelPath(subpath):
    for path in ['..', '../..', '/opt/openrobots/share/example-robot-data']:
        if exists(join(path, subpath.strip('/'))):
11
            print("using %s as modelPath" % path)
12
13
14
15
            return path
    raise IOError('%s not found' % (subpath))


16
def readParamsFromSrdf(robot, SRDF_PATH, verbose):
Guilhem Saurel's avatar
Guilhem Saurel committed
17
18
19
    rmodel = robot.model

    pinocchio.loadRotorParameters(rmodel, SRDF_PATH, verbose)
20
21
22
    rmodel.armature = np.multiply(rmodel.rotorInertia.flat, np.square(rmodel.rotorGearRatio.flat))
    pinocchio.loadReferenceConfigurations(rmodel, SRDF_PATH, verbose)
    robot.q0.flat[:] = rmodel.referenceConfigurations["half_sitting"].copy()
Guilhem Saurel's avatar
Guilhem Saurel committed
23
24
25
    return


26
def loadTalosArm():
27
    URDF_FILENAME = "talos_left_arm.urdf"
28
    URDF_SUBPATH = "/talos_data/robots/" + URDF_FILENAME
29
    SRDF_FILENAME = "talos.srdf"
30
    SRDF_SUBPATH = "/talos_data/srdf/" + SRDF_FILENAME
31
    modelPath = getModelPath(URDF_SUBPATH)
32
    # Load URDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
33
    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
34
35

    # Load SRDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
36
    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False)
37
38
    return robot

Guilhem Saurel's avatar
Guilhem Saurel committed
39

40
def loadTalos():
41
42
    URDF_FILENAME = "talos_reduced.urdf"
    SRDF_FILENAME = "talos.srdf"
43
44
    SRDF_SUBPATH = "/talos_data/srdf/" + SRDF_FILENAME
    URDF_SUBPATH = "/talos_data/robots/" + URDF_FILENAME
45
    modelPath = getModelPath(URDF_SUBPATH)
46
    # Load URDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
47
    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
48
    # Load SRDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
49
50
    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False)
    assert ((robot.model.armature[:6] == 0.).all())
51
52
53
    return robot


54
def loadHyQ():
55
    URDF_FILENAME = "hyq_no_sensors.urdf"
56
57
    SRDF_FILENAME = "hyq.srdf"
    SRDF_SUBPATH = "/hyq_description/srdf/" + SRDF_FILENAME
58
    URDF_SUBPATH = "/hyq_description/robots/" + URDF_FILENAME
59
    modelPath = getModelPath(URDF_SUBPATH)
60
    # Load URDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
61
    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
62
    # Load SRDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
63
    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False)
64
    return robot
65
66
67
68


def loadTiago():
    URDF_FILENAME = "tiago.urdf"
Carlos Mastalli's avatar
Carlos Mastalli committed
69
70
    #    SRDF_FILENAME = "tiago.srdf"
    #    SRDF_SUBPATH = "/tiago_description/srdf/" + SRDF_FILENAME
71
72
73
    URDF_SUBPATH = "/tiago_description/robots/" + URDF_FILENAME
    modelPath = getModelPath(URDF_SUBPATH)
    # Load URDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
74
    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
75
    # Load SRDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
76
    #    readParamsFromSrdf(robot, modelPath+SRDF_SUBPATH, False)
77
78
79
80
81
    return robot


def loadTiagoNoHand():
    URDF_FILENAME = "tiago_no_hand.urdf"
Carlos Mastalli's avatar
Carlos Mastalli committed
82
83
    #    SRDF_FILENAME = "tiago.srdf"
    #    SRDF_SUBPATH = "/tiago_description/srdf/" + SRDF_FILENAME
84
85
86
    URDF_SUBPATH = "/tiago_description/robots/" + URDF_FILENAME
    modelPath = getModelPath(URDF_SUBPATH)
    # Load URDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
87
    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
88
    # Load SRDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
89
    #    readParamsFromSrdf(robot, modelPath+SRDF_SUBPATH, False)
90
    return robot
91

Carlos Mastalli's avatar
Carlos Mastalli committed
92

93
94
95
96
97
def loadICub(reduced=True):
    if reduced:
        URDF_FILENAME = "icub_reduced.urdf"
    else:
        URDF_FILENAME = "icub.urdf"
98
99
    SRDF_FILENAME = "icub.srdf"
    SRDF_SUBPATH = "/icub_description/srdf/" + SRDF_FILENAME
100
101
102
    URDF_SUBPATH = "/icub_description/robots/" + URDF_FILENAME
    modelPath = getModelPath(URDF_SUBPATH)
    # Load URDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
103
    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
104
    # Load SRDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
105
    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False)
106
    return robot