robots_loader.py 6.94 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
18
    for path in (dirname(dirname(dirname(__file__))), dirname(dirname(__file__))):
        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):
Guilhem Saurel's avatar
Guilhem Saurel committed
26
27
28
    rmodel = robot.model

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


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

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


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

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

Guilhem Saurel's avatar
Guilhem Saurel committed
59

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

Guilhem Saurel's avatar
Guilhem Saurel committed
75

Carlos Mastalli's avatar
Carlos Mastalli committed
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
129
130
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

131

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


Carlos Mastalli's avatar
Carlos Mastalli committed
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
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


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


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

Carlos Mastalli's avatar
Carlos Mastalli committed
190

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