robots_loader.py 8.53 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):
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 32
    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
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 51 52 53
def loadANYmal(withArm=None):
    if withArm is None:
        URDF_FILENAME = "anymal.urdf"
        SRDF_FILENAME = "anymal.srdf"
    elif withArm is "kinova":
        URDF_FILENAME = "anymal-kinova.urdf"
        SRDF_FILENAME = "anymal-kinova.srdf"
54 55 56 57 58 59 60 61 62 63 64 65
    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
    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False)
    # Add the free-flyer joint limits
    addFreeFlyerJointLimits(robot)
    return robot


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

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

Guilhem Saurel's avatar
Guilhem Saurel committed
79

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

Guilhem Saurel's avatar
Guilhem Saurel committed
95

Carlos Mastalli's avatar
Carlos Mastalli committed
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 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150
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

151

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


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


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


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


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

Carlos Mastalli's avatar
Carlos Mastalli committed
223

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


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