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

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


9
def getModelPath(subpath):
10
11
12
    base = '../../../share/example-robot-data'
    for p in sys.path:
        path = join(p, base.strip('/'))
13
        if exists(join(path, subpath.strip('/'))):
14
            print("using %s as modelPath" % path)
15
16
17
18
            return path
    raise IOError('%s not found' % (subpath))


19
def readParamsFromSrdf(robot, SRDF_PATH, verbose):
Guilhem Saurel's avatar
Guilhem Saurel committed
20
21
22
    rmodel = robot.model

    pinocchio.loadRotorParameters(rmodel, SRDF_PATH, verbose)
23
24
25
    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
26
27
28
    return


29
30
31
32
33
34
35
36
37
38
39
def addFreeFlyerJointLimits(robot):
    rmodel = robot.model

    ub = rmodel.upperPositionLimit
    ub[:7] = 1
    rmodel.upperPositionLimit = ub
    lb = rmodel.lowerPositionLimit
    lb[:7] = -1
    rmodel.lowerPositionLimit = lb


40
def loadTalosArm():
41
    URDF_FILENAME = "talos_left_arm.urdf"
42
    URDF_SUBPATH = "/talos_data/robots/" + URDF_FILENAME
43
    SRDF_FILENAME = "talos.srdf"
44
    SRDF_SUBPATH = "/talos_data/srdf/" + SRDF_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])
48
49

    # Load SRDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
50
    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False)
51
52
    return robot

Guilhem Saurel's avatar
Guilhem Saurel committed
53

54
def loadTalos():
55
56
    URDF_FILENAME = "talos_reduced.urdf"
    SRDF_FILENAME = "talos.srdf"
57
58
    SRDF_SUBPATH = "/talos_data/srdf/" + SRDF_FILENAME
    URDF_SUBPATH = "/talos_data/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
64
    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False)
    assert ((robot.model.armature[:6] == 0.).all())
65
    # Add the free-flyer joint limits
66
    addFreeFlyerJointLimits(robot)
67
68
69
    return robot


70
def loadHyQ():
71
    URDF_FILENAME = "hyq_no_sensors.urdf"
72
73
    SRDF_FILENAME = "hyq.srdf"
    SRDF_SUBPATH = "/hyq_description/srdf/" + SRDF_FILENAME
74
    URDF_SUBPATH = "/hyq_description/robots/" + URDF_FILENAME
75
    modelPath = getModelPath(URDF_SUBPATH)
76
    # Load URDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
77
    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
78
    # Load SRDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
79
    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False)
80
81
    # Add the free-flyer joint limits
    addFreeFlyerJointLimits(robot)
82
    return robot
83
84
85
86


def loadTiago():
    URDF_FILENAME = "tiago.urdf"
Carlos Mastalli's avatar
Carlos Mastalli committed
87
88
    #    SRDF_FILENAME = "tiago.srdf"
    #    SRDF_SUBPATH = "/tiago_description/srdf/" + SRDF_FILENAME
89
90
91
    URDF_SUBPATH = "/tiago_description/robots/" + URDF_FILENAME
    modelPath = getModelPath(URDF_SUBPATH)
    # Load URDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
92
    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
93
    # Load SRDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
94
    #    readParamsFromSrdf(robot, modelPath+SRDF_SUBPATH, False)
95
96
97
98
99
    return robot


def loadTiagoNoHand():
    URDF_FILENAME = "tiago_no_hand.urdf"
Carlos Mastalli's avatar
Carlos Mastalli committed
100
101
    #    SRDF_FILENAME = "tiago.srdf"
    #    SRDF_SUBPATH = "/tiago_description/srdf/" + SRDF_FILENAME
102
103
104
    URDF_SUBPATH = "/tiago_description/robots/" + URDF_FILENAME
    modelPath = getModelPath(URDF_SUBPATH)
    # Load URDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
105
    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
106
    # Load SRDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
107
    #    readParamsFromSrdf(robot, modelPath+SRDF_SUBPATH, False)
108
    return robot
109

Carlos Mastalli's avatar
Carlos Mastalli committed
110

111
112
113
114
115
def loadICub(reduced=True):
    if reduced:
        URDF_FILENAME = "icub_reduced.urdf"
    else:
        URDF_FILENAME = "icub.urdf"
116
117
    SRDF_FILENAME = "icub.srdf"
    SRDF_SUBPATH = "/icub_description/srdf/" + SRDF_FILENAME
118
119
120
    URDF_SUBPATH = "/icub_description/robots/" + URDF_FILENAME
    modelPath = getModelPath(URDF_SUBPATH)
    # Load URDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
121
    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
122
    # Load SRDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
123
    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False)
124
125
    # Add the free-flyer joint limits
    addFreeFlyerJointLimits(robot)
126
    return robot