robots_loader.py 8.53 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
def loadANYmal(withArm=None):
    if withArm is None:
        URDF_FILENAME = "anymal.urdf"
        SRDF_FILENAME = "anymal.srdf"
    elif withArm is "kinova":
        URDF_FILENAME = "anymal-kinova.urdf"
        SRDF_FILENAME = "anymal-kinova.srdf"
54
55
56
57
58
59
60
61
62
63
64
65
    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
    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


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

    # Load SRDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
76
    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False)
77
78
    return robot

Guilhem Saurel's avatar
Guilhem Saurel committed
79

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

Guilhem Saurel's avatar
Guilhem Saurel committed
95

Carlos Mastalli's avatar
Carlos Mastalli committed
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
147
148
149
150
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

151

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


Carlos Mastalli's avatar
Carlos Mastalli committed
167
168
169
170
171
172
173
174
175
176
177
178
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
179
    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False)
Carlos Mastalli's avatar
Carlos Mastalli committed
180
181
182
183
184
    # Add the free-flyer joint limits
    addFreeFlyerJointLimits(robot)
    return robot


185
186
187
188
189
190
191
192
193
194
195
196
197
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


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


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

Carlos Mastalli's avatar
Carlos Mastalli committed
223

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


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])