robots_loader.py 6.87 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):
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
            print("using %s as modelPath" % path)
15
            return path
16
17
18
19
    for path in (dirname(dirname(dirname(__file__))), dirname(dirname(__file__))):
        if exists(join(path, subpath.strip('/'))):
            print("using %s as modelPath" % path)
            return path
20
21
22
    raise IOError('%s not found' % (subpath))


23
def readParamsFromSrdf(robot, SRDF_PATH, verbose):
Guilhem Saurel's avatar
Guilhem Saurel committed
24
25
26
    rmodel = robot.model

    pinocchio.loadRotorParameters(rmodel, SRDF_PATH, verbose)
27
28
29
    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
30
31
32
    return


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

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


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

    # Load SRDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
54
    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False)
55
56
    return robot

Guilhem Saurel's avatar
Guilhem Saurel committed
57

58
def loadTalos():
59
60
    URDF_FILENAME = "talos_reduced.urdf"
    SRDF_FILENAME = "talos.srdf"
61
62
    SRDF_SUBPATH = "/talos_data/srdf/" + SRDF_FILENAME
    URDF_SUBPATH = "/talos_data/robots/" + URDF_FILENAME
63
    modelPath = getModelPath(URDF_SUBPATH)
64
    # Load URDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
65
    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
66
    # Load SRDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
67
68
    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False)
    assert ((robot.model.armature[:6] == 0.).all())
69
    # Add the free-flyer joint limits
70
    addFreeFlyerJointLimits(robot)
71
72
    return robot

Guilhem Saurel's avatar
Guilhem Saurel committed
73

Carlos Mastalli's avatar
Carlos Mastalli committed
74
75
76
77
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
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

129

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


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


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


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

Carlos Mastalli's avatar
Carlos Mastalli committed
188

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