robots_loader.py 7.32 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
9
import pinocchio
from pinocchio.robot_wrapper import RobotWrapper


10
def getModelPath(subpath, printmsg=False):
11
12
13
    base = '../../../share/example-robot-data'
    for p in sys.path:
        path = join(p, base.strip('/'))
14
        if exists(join(path, subpath.strip('/'))):
15
16
            if printmsg:
                print("using %s as modelPath" % path)
17
            return path
18
19
    for path in (dirname(dirname(dirname(__file__))), dirname(dirname(__file__))):
        if exists(join(path, subpath.strip('/'))):
20
21
            if printmsg:
                print("using %s as modelPath" % path)
22
            return path
23
24
25
    raise IOError('%s not found' % (subpath))


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

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


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

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


48
def loadTalosArm():
49
    URDF_FILENAME = "talos_left_arm.urdf"
50
    URDF_SUBPATH = "/talos_data/robots/" + URDF_FILENAME
51
    SRDF_FILENAME = "talos.srdf"
52
    SRDF_SUBPATH = "/talos_data/srdf/" + SRDF_FILENAME
53
    modelPath = getModelPath(URDF_SUBPATH)
54
    # Load URDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
55
    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
56
57

    # Load SRDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
58
    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False)
59
60
    return robot

Guilhem Saurel's avatar
Guilhem Saurel committed
61

62
def loadTalos():
63
64
    URDF_FILENAME = "talos_reduced.urdf"
    SRDF_FILENAME = "talos.srdf"
65
66
    SRDF_SUBPATH = "/talos_data/srdf/" + SRDF_FILENAME
    URDF_SUBPATH = "/talos_data/robots/" + URDF_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], pinocchio.JointModelFreeFlyer())
70
    # Load SRDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
71
72
    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False)
    assert ((robot.model.armature[:6] == 0.).all())
73
    # Add the free-flyer joint limits
74
    addFreeFlyerJointLimits(robot)
75
76
    return robot

Guilhem Saurel's avatar
Guilhem Saurel committed
77

Carlos Mastalli's avatar
Carlos Mastalli committed
78
79
80
81
82
83
84
85
86
87
88
89
90
91
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
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

133

134
def loadHyQ():
135
    URDF_FILENAME = "hyq_no_sensors.urdf"
136
137
    SRDF_FILENAME = "hyq.srdf"
    SRDF_SUBPATH = "/hyq_description/srdf/" + SRDF_FILENAME
138
    URDF_SUBPATH = "/hyq_description/robots/" + URDF_FILENAME
139
    modelPath = getModelPath(URDF_SUBPATH)
140
    # Load URDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
141
    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
142
    # Load SRDF file
143
    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False)
144
145
    # Add the free-flyer joint limits
    addFreeFlyerJointLimits(robot)
146
    return robot
147
148


Carlos Mastalli's avatar
Carlos Mastalli committed
149
150
151
152
153
154
155
156
157
158
159
160
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
161
    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False)
Carlos Mastalli's avatar
Carlos Mastalli committed
162
163
164
165
166
    # Add the free-flyer joint limits
    addFreeFlyerJointLimits(robot)
    return robot


167
168
def loadTiago():
    URDF_FILENAME = "tiago.urdf"
Carlos Mastalli's avatar
Carlos Mastalli committed
169
170
    #    SRDF_FILENAME = "tiago.srdf"
    #    SRDF_SUBPATH = "/tiago_description/srdf/" + SRDF_FILENAME
171
172
173
    URDF_SUBPATH = "/tiago_description/robots/" + URDF_FILENAME
    modelPath = getModelPath(URDF_SUBPATH)
    # Load URDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
174
    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
175
    # Load SRDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
176
    #    readParamsFromSrdf(robot, modelPath+SRDF_SUBPATH, False)
177
178
179
180
181
    return robot


def loadTiagoNoHand():
    URDF_FILENAME = "tiago_no_hand.urdf"
Carlos Mastalli's avatar
Carlos Mastalli committed
182
183
    #    SRDF_FILENAME = "tiago.srdf"
    #    SRDF_SUBPATH = "/tiago_description/srdf/" + SRDF_FILENAME
184
185
186
    URDF_SUBPATH = "/tiago_description/robots/" + URDF_FILENAME
    modelPath = getModelPath(URDF_SUBPATH)
    # Load URDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
187
    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
188
    # Load SRDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
189
    #    readParamsFromSrdf(robot, modelPath+SRDF_SUBPATH, False)
190
    return robot
191

Carlos Mastalli's avatar
Carlos Mastalli committed
192

193
194
195
196
197
def loadICub(reduced=True):
    if reduced:
        URDF_FILENAME = "icub_reduced.urdf"
    else:
        URDF_FILENAME = "icub.urdf"
198
199
    SRDF_FILENAME = "icub.srdf"
    SRDF_SUBPATH = "/icub_description/srdf/" + SRDF_FILENAME
200
201
202
    URDF_SUBPATH = "/icub_description/robots/" + URDF_FILENAME
    modelPath = getModelPath(URDF_SUBPATH)
    # Load URDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
203
    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
204
    # Load SRDF file
205
    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False)
206
207
    # Add the free-flyer joint limits
    addFreeFlyerJointLimits(robot)
208
    return robot
Guilhem Saurel's avatar
Guilhem Saurel committed
209
210
211
212
213
214
215


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