robots_loader.py 9.47 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
    base = '../../../share/example-robot-data'
11
12
    for path in [dirname(dirname(dirname(dirname(__file__)))),
                 dirname(dirname(dirname(__file__)))] + [join(p, base.strip('/')) for p in sys.path]:
13
        if exists(join(path, subpath.strip('/'))):
14
15
            if printmsg:
                print("using %s as modelPath" % path)
16
            return path
17
18
19
    raise IOError('%s not found' % (subpath))


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

23
24
    if has_rotor_parameters:
        pinocchio.loadRotorParameters(rmodel, SRDF_PATH, verbose)
25
    rmodel.armature = np.multiply(rmodel.rotorInertia.flat, np.square(rmodel.rotorGearRatio.flat))
26
    pinocchio.loadReferenceConfigurations(rmodel, SRDF_PATH, verbose)
27
    robot.q0.flat[:] = rmodel.referenceConfigurations[referencePose].copy()
Guilhem Saurel's avatar
Guilhem Saurel committed
28
29
30
    return


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

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


42
43
44
45
def loadANYmal(withArm=None):
    if withArm is None:
        URDF_FILENAME = "anymal.urdf"
        SRDF_FILENAME = "anymal.srdf"
46
        REF_POSTURE = "standing"
Carlos Mastalli's avatar
Carlos Mastalli committed
47
    elif withArm == "kinova":
48
49
        URDF_FILENAME = "anymal-kinova.urdf"
        SRDF_FILENAME = "anymal-kinova.srdf"
50
        REF_POSTURE = "standing_with_arm_up"
51
52
53
54
    URDF_SUBPATH = "/anymal_b_simple_description/robots/" + URDF_FILENAME
    SRDF_SUBPATH = "/anymal_b_simple_description/srdf/" + SRDF_FILENAME
    modelPath = getModelPath(URDF_SUBPATH)
    # Load URDF file
55
    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
56
    # Load SRDF file
57
    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False, referencePose=REF_POSTURE)
58
59
60
61
62
    # Add the free-flyer joint limits
    addFreeFlyerJointLimits(robot)
    return robot


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

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

Guilhem Saurel's avatar
Guilhem Saurel committed
76

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

Guilhem Saurel's avatar
Guilhem Saurel committed
92

Carlos Mastalli's avatar
Carlos Mastalli committed
93
94
95
96
97
98
99
100
101
102
103
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()
104
    for j, M, name, parent, Y in zip(m1.joints, m1.jointPlacements, m1.names, m1.parents, m1.inertias):
Carlos Mastalli's avatar
Carlos Mastalli committed
105
        if j.id < legMaxId:
106
            jid = m2.addJoint(parent, getattr(pinocchio, j.shortname())(), M, name)
Carlos Mastalli's avatar
Carlos Mastalli committed
107
108
            up = m2.upperPositionLimit
            down = m2.lowerPositionLimit
109
110
            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]
Carlos Mastalli's avatar
Carlos Mastalli committed
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
147
            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

148

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


Carlos Mastalli's avatar
Carlos Mastalli committed
164
165
166
167
168
169
170
171
172
173
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
174
    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
Carlos Mastalli's avatar
Carlos Mastalli committed
175
    # Load SRDF file
176
    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False, "standing")
Carlos Mastalli's avatar
Carlos Mastalli committed
177
178
179
180
181
    # Add the free-flyer joint limits
    addFreeFlyerJointLimits(robot)
    return robot


182
183
184
185
186
187
188
189
190
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
191
    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False, "arm_up")
192
193
194
    return robot


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


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

Carlos Mastalli's avatar
Carlos Mastalli committed
220

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


239
240
def loadUR(robot=5, limited=False, gripper=False):
    assert (not (gripper and (robot == 10 or limited)))
241
    URDF_FILENAME = "ur%i%s_%s.urdf" % (robot, "_joint_limited" if limited else '', 'gripper' if gripper else 'robot')
242
    URDF_SUBPATH = "/ur_description/urdf/" + URDF_FILENAME
Guilhem Saurel's avatar
Guilhem Saurel committed
243
244
    modelPath = getModelPath(URDF_SUBPATH)
    return RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
245
246
247
248
249
250


def loadHector():
    URDF_FILENAME = "quadrotor_base.urdf"
    URDF_SUBPATH = "/hector_description/robots/" + URDF_FILENAME
    modelPath = getModelPath(URDF_SUBPATH)
251
    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
252
    return robot
253
254


Pep Marti Saumell's avatar
Pep Marti Saumell committed
255
256
257
def loadDoublePendulum():
    URDF_FILENAME = "double_pendulum.urdf"
    URDF_SUBPATH = "/double_pendulum_description/urdf/" + URDF_FILENAME
258
259
260
    modelPath = getModelPath(URDF_SUBPATH)
    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
    return robot
Pep Marti Saumell's avatar
Pep Marti Saumell committed
261
262


Guilhem Saurel's avatar
Guilhem Saurel committed
263
264
265
266
def loadRomeo():
    URDF_FILENAME = "romeo.urdf"
    URDF_SUBPATH = "/romeo_description/urdf/" + URDF_FILENAME
    modelPath = getModelPath(URDF_SUBPATH)
267
    return RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())