robots_loader.py 6.04 KB
Newer Older
1
2
from os.path import exists, join

3
import sys
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
16
17
18
            return path
    raise IOError('%s not found' % (subpath))


19
def readParamsFromSrdf(robot, SRDF_PATH, verbose):
Guilhem Saurel's avatar
Guilhem Saurel committed
20
21
22
    rmodel = robot.model

    pinocchio.loadRotorParameters(rmodel, SRDF_PATH, verbose)
23
24
25
    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
26
27
28
    return


29
30
31
32
33
34
35
36
37
38
39
def addFreeFlyerJointLimits(robot):
    rmodel = robot.model

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


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

    # Load SRDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
50
    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False)
51
52
    return robot

Guilhem Saurel's avatar
Guilhem Saurel committed
53

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

Carlos Mastalli's avatar
Carlos Mastalli committed
69
70
71
72
73
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
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

124

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


def loadTiago():
    URDF_FILENAME = "tiago.urdf"
Carlos Mastalli's avatar
Carlos Mastalli committed
142
143
    #    SRDF_FILENAME = "tiago.srdf"
    #    SRDF_SUBPATH = "/tiago_description/srdf/" + SRDF_FILENAME
144
145
146
    URDF_SUBPATH = "/tiago_description/robots/" + URDF_FILENAME
    modelPath = getModelPath(URDF_SUBPATH)
    # Load URDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
147
    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
148
    # Load SRDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
149
    #    readParamsFromSrdf(robot, modelPath+SRDF_SUBPATH, False)
150
151
152
153
154
    return robot


def loadTiagoNoHand():
    URDF_FILENAME = "tiago_no_hand.urdf"
Carlos Mastalli's avatar
Carlos Mastalli committed
155
156
    #    SRDF_FILENAME = "tiago.srdf"
    #    SRDF_SUBPATH = "/tiago_description/srdf/" + SRDF_FILENAME
157
158
159
    URDF_SUBPATH = "/tiago_description/robots/" + URDF_FILENAME
    modelPath = getModelPath(URDF_SUBPATH)
    # Load URDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
160
    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
161
    # Load SRDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
162
    #    readParamsFromSrdf(robot, modelPath+SRDF_SUBPATH, False)
163
    return robot
164

Carlos Mastalli's avatar
Carlos Mastalli committed
165

166
167
168
169
170
def loadICub(reduced=True):
    if reduced:
        URDF_FILENAME = "icub_reduced.urdf"
    else:
        URDF_FILENAME = "icub.urdf"
171
172
    SRDF_FILENAME = "icub.srdf"
    SRDF_SUBPATH = "/icub_description/srdf/" + SRDF_FILENAME
173
174
175
    URDF_SUBPATH = "/icub_description/robots/" + URDF_FILENAME
    modelPath = getModelPath(URDF_SUBPATH)
    # Load URDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
176
    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
177
    # Load SRDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
178
    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False)
179
180
    # Add the free-flyer joint limits
    addFreeFlyerJointLimits(robot)
181
    return robot