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

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, printmsg=False):
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
15
            if printmsg:
                print("using %s as modelPath" % path)
16
            return path
17
    for path in (dirname(dirname(dirname(dirname(__file__)))), dirname(dirname(dirname(__file__)))):
18
        if exists(join(path, subpath.strip('/'))):
19
20
            if printmsg:
                print("using %s as modelPath" % path)
21
            return path
22
23
24
    raise IOError('%s not found' % (subpath))


25
def readParamsFromSrdf(robot, SRDF_PATH, verbose, has_rotor_parameters=True):
Guilhem Saurel's avatar
Guilhem Saurel committed
26
27
    rmodel = robot.model

28
29
    if has_rotor_parameters:
        pinocchio.loadRotorParameters(rmodel, SRDF_PATH, verbose)
30
31
32
    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
33
34
35
    return


36
37
38
39
40
41
42
43
44
45
46
def addFreeFlyerJointLimits(robot):
    rmodel = robot.model

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


47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
def loadANYmal():
    URDF_FILENAME = "anymal.urdf"
    URDF_SUBPATH = "/anymal_b_simple_description/robots/" + URDF_FILENAME
    SRDF_FILENAME = "anymal.srdf"
    SRDF_SUBPATH = "/anymal_b_simple_description/srdf/" + SRDF_FILENAME
    modelPath = getModelPath(URDF_SUBPATH)
    # Load URDF file
    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
    # Load SRDF file
    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False)
    # Add the free-flyer joint limits
    addFreeFlyerJointLimits(robot)
    return robot


62
def loadTalosArm():
63
    URDF_FILENAME = "talos_left_arm.urdf"
64
    URDF_SUBPATH = "/talos_data/robots/" + URDF_FILENAME
65
    SRDF_FILENAME = "talos.srdf"
66
    SRDF_SUBPATH = "/talos_data/srdf/" + SRDF_FILENAME
67
    modelPath = getModelPath(URDF_SUBPATH)
68
    # Load URDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
69
    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
70
71

    # Load SRDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
72
    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False)
73
74
    return robot

Guilhem Saurel's avatar
Guilhem Saurel committed
75

76
def loadTalos():
77
78
    URDF_FILENAME = "talos_reduced.urdf"
    SRDF_FILENAME = "talos.srdf"
79
80
    SRDF_SUBPATH = "/talos_data/srdf/" + SRDF_FILENAME
    URDF_SUBPATH = "/talos_data/robots/" + URDF_FILENAME
81
    modelPath = getModelPath(URDF_SUBPATH)
82
    # Load URDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
83
    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
84
    # Load SRDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
85
86
    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False)
    assert ((robot.model.armature[:6] == 0.).all())
87
    # Add the free-flyer joint limits
88
    addFreeFlyerJointLimits(robot)
89
90
    return robot

Guilhem Saurel's avatar
Guilhem Saurel committed
91

Carlos Mastalli's avatar
Carlos Mastalli committed
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
120
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
def loadTalosLegs():
    robot = loadTalos()
    URDF_FILENAME = "talos_reduced.urdf"
    SRDF_FILENAME = "talos.srdf"
    SRDF_SUBPATH = "/talos_data/srdf/" + SRDF_FILENAME
    URDF_SUBPATH = "/talos_data/robots/" + URDF_FILENAME
    modelPath = getModelPath(URDF_SUBPATH)

    legMaxId = 14
    m1 = robot.model
    m2 = pinocchio.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(pinocchio, j.shortname())(), M, name)
            up = m2.upperPositionLimit
            down = m2.lowerPositionLimit
            up[m2.joints[jid].idx_q:m2.joints[jid].idx_q + j.nq] = m1.upperPositionLimit[j.idx_q:j.idx_q + j.nq]
            down[m2.joints[jid].idx_q:m2.joints[jid].idx_q + j.nq] = m1.lowerPositionLimit[j.idx_q:j.idx_q + j.nq]
            m2.upperPositionLimit = up
            m2.lowerPositionLimit = down
            assert (jid == j.id)
            m2.appendBodyToJoint(jid, Y, pinocchio.SE3.Identity())

    u = m2.upperPositionLimit
    u[:7] = 1
    m2.upperPositionLimit = u
    limit = m2.lowerPositionLimit
    limit[:7] = -1
    m2.lowerPositionLimit = limit

    # q2 = robot.q0[:19]
    for f in m1.frames:
        if f.parent < legMaxId:
            m2.addFrame(f)

    g2 = pinocchio.GeometryModel()
    for g in robot.visual_model.geometryObjects:
        if g.parentJoint < 14:
            g2.addGeometryObject(g)

    robot.model = m2
    robot.data = m2.createData()
    robot.visual_model = g2
    # robot.q0=q2
    robot.visual_data = pinocchio.GeometryData(g2)

    # Load SRDF file
    robot.q0 = np.matrix(np.resize(robot.q0, robot.model.nq)).T
    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False)

    assert ((m2.armature[:6] == 0.).all())
    # Add the free-flyer joint limits
    addFreeFlyerJointLimits(robot)
    return robot

147

148
def loadHyQ():
149
    URDF_FILENAME = "hyq_no_sensors.urdf"
150
151
    SRDF_FILENAME = "hyq.srdf"
    SRDF_SUBPATH = "/hyq_description/srdf/" + SRDF_FILENAME
152
    URDF_SUBPATH = "/hyq_description/robots/" + URDF_FILENAME
153
    modelPath = getModelPath(URDF_SUBPATH)
154
    # Load URDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
155
    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
156
    # Load SRDF file
157
    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False)
158
159
    # Add the free-flyer joint limits
    addFreeFlyerJointLimits(robot)
160
    return robot
161
162


Carlos Mastalli's avatar
Carlos Mastalli committed
163
164
165
166
167
168
169
170
171
172
173
174
def loadSolo(solo=True):
    if solo:
        URDF_FILENAME = "solo.urdf"
    else:
        URDF_FILENAME = "solo12.urdf"
    SRDF_FILENAME = "solo.srdf"
    SRDF_SUBPATH = "/solo_description/srdf/" + SRDF_FILENAME
    URDF_SUBPATH = "/solo_description/robots/" + URDF_FILENAME
    modelPath = getModelPath(URDF_SUBPATH)
    # Load URDF file
    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
    # Load SRDF file
175
    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False)
Carlos Mastalli's avatar
Carlos Mastalli committed
176
177
178
179
180
    # Add the free-flyer joint limits
    addFreeFlyerJointLimits(robot)
    return robot


181
182
183
184
185
186
187
188
189
190
191
192
193
def loadKinova():
    URDF_FILENAME = "kinova.urdf"
    SRDF_FILENAME = "kinova.srdf"
    SRDF_SUBPATH = "/kinova_description/srdf/" + SRDF_FILENAME
    URDF_SUBPATH = "/kinova_description/robots/" + URDF_FILENAME
    modelPath = getModelPath(URDF_SUBPATH)
    # Load URDF file
    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
    # Load SRDF file
    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False)
    return robot


194
195
def loadTiago():
    URDF_FILENAME = "tiago.urdf"
Carlos Mastalli's avatar
Carlos Mastalli committed
196
197
    #    SRDF_FILENAME = "tiago.srdf"
    #    SRDF_SUBPATH = "/tiago_description/srdf/" + SRDF_FILENAME
198
199
200
    URDF_SUBPATH = "/tiago_description/robots/" + URDF_FILENAME
    modelPath = getModelPath(URDF_SUBPATH)
    # Load URDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
201
    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
202
    # Load SRDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
203
    #    readParamsFromSrdf(robot, modelPath+SRDF_SUBPATH, False)
204
205
206
207
208
    return robot


def loadTiagoNoHand():
    URDF_FILENAME = "tiago_no_hand.urdf"
Carlos Mastalli's avatar
Carlos Mastalli committed
209
210
    #    SRDF_FILENAME = "tiago.srdf"
    #    SRDF_SUBPATH = "/tiago_description/srdf/" + SRDF_FILENAME
211
212
213
    URDF_SUBPATH = "/tiago_description/robots/" + URDF_FILENAME
    modelPath = getModelPath(URDF_SUBPATH)
    # Load URDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
214
    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
215
    # Load SRDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
216
    #    readParamsFromSrdf(robot, modelPath+SRDF_SUBPATH, False)
217
    return robot
218

Carlos Mastalli's avatar
Carlos Mastalli committed
219

220
221
222
223
224
def loadICub(reduced=True):
    if reduced:
        URDF_FILENAME = "icub_reduced.urdf"
    else:
        URDF_FILENAME = "icub.urdf"
225
226
    SRDF_FILENAME = "icub.srdf"
    SRDF_SUBPATH = "/icub_description/srdf/" + SRDF_FILENAME
227
228
229
    URDF_SUBPATH = "/icub_description/robots/" + URDF_FILENAME
    modelPath = getModelPath(URDF_SUBPATH)
    # Load URDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
230
    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
231
    # Load SRDF file
232
    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False)
233
234
    # Add the free-flyer joint limits
    addFreeFlyerJointLimits(robot)
235
    return robot
Guilhem Saurel's avatar
Guilhem Saurel committed
236
237
238
239
240
241
242


def loadUR(robot=5, limited=False):
    URDF_FILENAME = 'ur%i%s_robot.urdf' % (robot, '_joint_limited' if limited else '')
    URDF_SUBPATH = '/ur_description/urdf/' + URDF_FILENAME
    modelPath = getModelPath(URDF_SUBPATH)
    return RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])