robots_loader.py 8.68 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, referencePose='half_sitting'):
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
    rmodel.armature = np.multiply(rmodel.rotorInertia.flat, np.square(rmodel.rotorGearRatio.flat))
    pinocchio.loadReferenceConfigurations(rmodel, SRDF_PATH, verbose)
32
    robot.q0.flat[:] = rmodel.referenceConfigurations[referencePose].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
def loadANYmal(withArm=None):
    if withArm is None:
        URDF_FILENAME = "anymal.urdf"
        SRDF_FILENAME = "anymal.srdf"
51
        REF_POSTURE = "standing"
Carlos Mastalli's avatar
Carlos Mastalli committed
52
    elif withArm == "kinova":
53
54
        URDF_FILENAME = "anymal-kinova.urdf"
        SRDF_FILENAME = "anymal-kinova.srdf"
55
        REF_POSTURE = "standing_with_arm_up"
56
57
58
59
60
61
    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
62
    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False, REF_POSTURE)
63
64
65
66
67
    # Add the free-flyer joint limits
    addFreeFlyerJointLimits(robot)
    return robot


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

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

Guilhem Saurel's avatar
Guilhem Saurel committed
81

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

Guilhem Saurel's avatar
Guilhem Saurel committed
97

Carlos Mastalli's avatar
Carlos Mastalli committed
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
151
152
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

153

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


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


187
188
189
190
191
192
193
194
195
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
196
    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False, "arm_up")
197
198
199
    return robot


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


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

Carlos Mastalli's avatar
Carlos Mastalli committed
225

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


def loadUR(robot=5, limited=False):
245
246
    URDF_FILENAME = "ur%i%s_robot.urdf" % (robot, "_joint_limited" if limited else '')
    URDF_SUBPATH = "/ur_description/urdf/" + URDF_FILENAME
Guilhem Saurel's avatar
Guilhem Saurel committed
247
248
    modelPath = getModelPath(URDF_SUBPATH)
    return RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])