robots_loader.py 10 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

12
def getModelPath(subpath, printmsg=False):
13
    source = dirname(dirname(dirname(__file__)))  # top level source directory
14
    paths = [
15
16
17
        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
18
19
    ]
    try:
20
21
22
        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
23
24
25
26
    except ImportError:
        pass
    paths += [join(p, '../../../share/example-robot-data/robots') for p in sys.path]
    for path in paths:
27
        if exists(join(path, subpath.strip('/'))):
28
29
            if printmsg:
                print("using %s as modelPath" % path)
30
            return path
31
    raise IOError('%s not found' % subpath)
32
33


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

44

45
46
def addFreeFlyerJointLimits(model):
    ub = model.upperPositionLimit
47
    ub[:7] = 1
48
49
    model.upperPositionLimit = ub
    lb = model.lowerPositionLimit
50
    lb[:7] = -1
51
    model.lowerPositionLimit = lb
52
53


54
55
def robot_loader(path,
                 urdf_filename,
56
                 srdf_filename=None,
57
                 urdf_subpath='robots',
58
59
60
61
62
                 verbose=False,
                 has_rotor_parameters=False,
                 ref_posture='half_sitting',
                 free_flyer=False):
    """Helper function to load a robot."""
63
    urdf_path = join(path, urdf_subpath, urdf_filename)
64
65
66
67
68
    model_path = getModelPath(urdf_path)
    robot = RobotWrapper.BuildFromURDF(join(model_path, urdf_path), [join(model_path, '../..')],
                                       pin.JointModelFreeFlyer() if free_flyer else None)

    if srdf_filename is not None:
69
        robot.q0 = readParamsFromSrdf(robot.model, join(model_path, path, 'srdf', srdf_filename), verbose,
70
71
72
73
74
75
76
                                      has_rotor_parameters, ref_posture)
    if free_flyer:
        addFreeFlyerJointLimits(robot.model)

    return robot


77
78
79
80
def loadANYmal(withArm=None):
    if withArm is None:
        URDF_FILENAME = "anymal.urdf"
        SRDF_FILENAME = "anymal.srdf"
81
        REF_POSTURE = "standing"
Carlos Mastalli's avatar
Carlos Mastalli committed
82
    elif withArm == "kinova":
83
84
        URDF_FILENAME = "anymal-kinova.urdf"
        SRDF_FILENAME = "anymal-kinova.srdf"
85
        REF_POSTURE = "standing_with_arm_up"
86

87
88
    return robot_loader('anymal_b_simple_description',
                        URDF_FILENAME,
89
90
91
                        SRDF_FILENAME,
                        ref_posture=REF_POSTURE,
                        free_flyer=True)
92
93


94
def loadTalos(legs=False, arm=False, full=False, box=False):
Guilhem Saurel's avatar
Guilhem Saurel committed
95
96
97
98
    if arm:
        URDF_FILENAME = "talos_left_arm.urdf"
    elif full:
        URDF_FILENAME = "talos_full_v2.urdf"
99
100
        if box:
            URDF_FILENAME = "talos_full_v2_box.urdf"
Guilhem Saurel's avatar
Guilhem Saurel committed
101
102
    else:
        URDF_FILENAME = "talos_reduced.urdf"
103
104
        if box:
            URDF_FILENAME = "talos_reduced_box.urdf"
105
    SRDF_FILENAME = "talos.srdf"
106

107
    robot = robot_loader('talos_data', URDF_FILENAME, SRDF_FILENAME, free_flyer=not arm)
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128

    assert (robot.model.armature[:6] == 0.).all()

    if legs:
        legMaxId = 14
        m1 = robot.model
        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)
                upperPos = m2.upperPositionLimit
                lowerPos = m2.lowerPositionLimit
                effort = m2.effortLimit
                upperPos[m2.joints[jid].idx_q:m2.joints[jid].idx_q + j.nq] = m1.upperPositionLimit[j.idx_q:j.idx_q +
                                                                                                   j.nq]
                lowerPos[m2.joints[jid].idx_q:m2.joints[jid].idx_q + j.nq] = m1.lowerPositionLimit[j.idx_q:j.idx_q +
                                                                                                   j.nq]
                effort[m2.joints[jid].idx_v:m2.joints[jid].idx_v + j.nv] = m1.effortLimit[j.idx_v:j.idx_v + j.nv]
                m2.upperPositionLimit = upperPos
                m2.lowerPositionLimit = lowerPos
                m2.effortLimit = effort
129
                assert jid == j.id
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
                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

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

        g2 = pin.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 = pin.GeometryData(g2)

        # Load SRDF file
        robot.q0 = robot.q0[:robot.model.nq]
160
        model_path = getModelPath(join('talos_data/robots', URDF_FILENAME))
161
        robot.q0 = readParamsFromSrdf(robot.model, join(model_path, 'talos_data/srdf', SRDF_FILENAME), False)
162

163
        assert (m2.armature[:6] == 0.).all()
164
165
166
        # Add the free-flyer joint limits to the new model
        addFreeFlyerJointLimits(robot.model)

167
168
    return robot

Guilhem Saurel's avatar
Guilhem Saurel committed
169

Carlos Mastalli's avatar
Carlos Mastalli committed
170
def loadTalosLegs():
171
172
    warnings.warn("`loadTalosLegs()` is deprecated. Please use `loadTalos(legs=True)`", DeprecationWarning, 2)
    return loadTalos(legs=True)
Carlos Mastalli's avatar
Carlos Mastalli committed
173

174

175
176
177
178
179
def loadTalosArm():
    warnings.warn("`loadTalosArm()` is deprecated. Please use `loadTalos(arm=True)`", DeprecationWarning, 2)
    return loadTalos(arm=True)


180
def loadHyQ():
Guilhem Saurel's avatar
Guilhem Saurel committed
181
    return robot_loader('hyq_description', "hyq_no_sensors.urdf", "hyq.srdf", ref_posture="standing", free_flyer=True)
182
183


Carlos Mastalli's avatar
Carlos Mastalli committed
184
def loadSolo(solo=True):
Guilhem Saurel's avatar
Guilhem Saurel committed
185
186
187
188
189
    return robot_loader('solo_description',
                        "solo.urdf" if solo else "solo12.urdf",
                        "solo.srdf",
                        ref_posture="standing",
                        free_flyer=True)
Carlos Mastalli's avatar
Carlos Mastalli committed
190
191


192
def loadKinova():
Guilhem Saurel's avatar
Guilhem Saurel committed
193
    return robot_loader('kinova_description', "kinova.urdf", "kinova.srdf", ref_posture="arm_up")
194

195

Guilhem Saurel's avatar
Guilhem Saurel committed
196
197
def loadTiago(hand=True):
    return robot_loader('tiago_description', "tiago.urdf" if hand else "tiago_no_hand.urdf")
198
199
200


def loadTiagoNoHand():
Guilhem Saurel's avatar
Guilhem Saurel committed
201
202
    warnings.warn("`loadTiagoNoHand()` is deprecated. Please use `loadTiago(hand=False)`", DeprecationWarning, 2)
    return loadTiago(hand=False)
203

Carlos Mastalli's avatar
Carlos Mastalli committed
204

205
def loadICub(reduced=True):
Guilhem Saurel's avatar
Guilhem Saurel committed
206
207
208
209
    return robot_loader('icub_description',
                        "icub_reduced.urdf" if reduced else "icub.urdf",
                        "icub.srdf",
                        free_flyer=True)
Guilhem Saurel's avatar
Guilhem Saurel committed
210
211


Justin Carpentier's avatar
Justin Carpentier committed
212
def loadPanda():
Guilhem Saurel's avatar
Guilhem Saurel committed
213
    return robot_loader('panda_description', "panda.urdf", urdf_subpath='urdf')
Justin Carpentier's avatar
Justin Carpentier committed
214

215

216
def loadUR(robot=5, limited=False, gripper=False):
217
    assert not (gripper and (robot == 10 or limited))
218
    URDF_FILENAME = "ur%i%s_%s.urdf" % (robot, "_joint_limited" if limited else '', 'gripper' if gripper else 'robot')
Guilhem Saurel's avatar
Guilhem Saurel committed
219
220
    if robot == 5 or robot == 3 and gripper:
        SRDF_FILENAME = "ur%i%s.srdf" % (robot, '_gripper' if gripper else '')
221
222
223
    else:
        SRDF_FILENAME = None

224
    return robot_loader('ur_description', URDF_FILENAME, SRDF_FILENAME, urdf_subpath='urdf', ref_posture=None)
225
226
227


def loadHector():
Guilhem Saurel's avatar
Guilhem Saurel committed
228
    return robot_loader('hector_description', "quadrotor_base.urdf", free_flyer=True)
229
230


Pep Marti Saumell's avatar
Pep Marti Saumell committed
231
def loadDoublePendulum():
Guilhem Saurel's avatar
Guilhem Saurel committed
232
    return robot_loader('double_pendulum_description', "double_pendulum.urdf", urdf_subpath='urdf')
Pep Marti Saumell's avatar
Pep Marti Saumell committed
233
234


Guilhem Saurel's avatar
Guilhem Saurel committed
235
def loadRomeo():
Guilhem Saurel's avatar
Guilhem Saurel committed
236
    return robot_loader('romeo_description', "romeo.urdf", urdf_subpath='urdf', free_flyer=True)
237

238

239
def loadIris():
Guilhem Saurel's avatar
Guilhem Saurel committed
240
    return robot_loader('iris_description', "iris_simple.urdf", free_flyer=True)
241
242
243
244
245
246
247
248
249
250


ROBOTS = {
    'anymal': (loadANYmal, {}),
    'anymal_kinova': (loadANYmal, {
        'withArm': 'kinova'
    }),
    'double_pendulum': (loadDoublePendulum, {}),
    'hector': (loadHector, {}),
    'hyq': (loadHyQ, {}),
Guilhem Saurel's avatar
Guilhem Saurel committed
251
252
253
254
255
256
    'icub': (loadICub, {
        'reduced': False
    }),
    'icub_reduced': (loadICub, {
        'reduced': True
    }),
257
258
259
260
261
262
263
264
265
    'iris': (loadIris, {}),
    'kinova': (loadKinova, {}),
    'panda': (loadPanda, {}),
    'romeo': (loadRomeo, {}),
    'solo': (loadSolo, {}),
    'solo12': (loadSolo, {
        'solo': False
    }),
    'talos': (loadTalos, {}),
266
    'talos_box': (loadTalos, {
Guilhem Saurel's avatar
Guilhem Saurel committed
267
        'box': True
268
    }),
269
270
271
272
273
274
    'talos_arm': (loadTalos, {
        'arm': True
    }),
    'talos_legs': (loadTalos, {
        'legs': True
    }),
Guilhem Saurel's avatar
Guilhem Saurel committed
275
276
277
    'talos_full': (loadTalos, {
        'full': True
    }),
278
279
    'talos_full_box': (loadTalos, {
        'full': True,
Guilhem Saurel's avatar
Guilhem Saurel committed
280
        'box': True
281
    }),
282
283
284
285
286
    'tiago': (loadTiago, {}),
    'tiago_no_hand': (loadTiago, {
        'hand': False
    }),
    'ur5': (loadUR, {}),
Guilhem Saurel's avatar
Guilhem Saurel committed
287
288
289
290
291
292
    'ur5_gripper': (loadUR, {
        'gripper': True
    }),
    'ur5_limited': (loadUR, {
        'limited': True
    }),
293
294
295
296
297
298
299
300
301
302
303
}


def load(name, display=False):
    """Load a robot by its name, and optionnaly display it in a viewer."""
    loader, kwargs = ROBOTS[name]
    robot = loader(**kwargs)
    if display:
        robot.initViewer(loadModel=True)
        robot.display(robot.q0)
    return robot