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

Guilhem Saurel's avatar
Guilhem Saurel committed
3
import numpy as np
4
5
6
7
import pinocchio
from pinocchio.robot_wrapper import RobotWrapper


8
9
10
def getModelPath(subpath):
    for path in ['..', '../..', '/opt/openrobots/share/example-robot-data']:
        if exists(join(path, subpath.strip('/'))):
11
            print("using %s as modelPath" % path)
12
13
14
15
            return path
    raise IOError('%s not found' % (subpath))


16
def readParamsFromSrdf(robot, SRDF_PATH, verbose):
Guilhem Saurel's avatar
Guilhem Saurel committed
17
18
19
    rmodel = robot.model

    pinocchio.loadRotorParameters(rmodel, SRDF_PATH, verbose)
20
21
22
    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
23
24
25
    return


26
27
28
29
30
31
32
33
34
35
36
def addFreeFlyerJointLimits(robot):
    rmodel = robot.model

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


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

    # Load SRDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
47
    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False)
48
49
    return robot

Guilhem Saurel's avatar
Guilhem Saurel committed
50

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


67
def loadHyQ():
68
    URDF_FILENAME = "hyq_no_sensors.urdf"
69
70
    SRDF_FILENAME = "hyq.srdf"
    SRDF_SUBPATH = "/hyq_description/srdf/" + SRDF_FILENAME
71
    URDF_SUBPATH = "/hyq_description/robots/" + URDF_FILENAME
72
    modelPath = getModelPath(URDF_SUBPATH)
73
    # Load URDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
74
    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
75
    # Load SRDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
76
    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False)
77
78
    # Add the free-flyer joint limits
    addFreeFlyerJointLimits(robot)
79
    return robot
80
81
82
83


def loadTiago():
    URDF_FILENAME = "tiago.urdf"
Carlos Mastalli's avatar
Carlos Mastalli committed
84
85
    #    SRDF_FILENAME = "tiago.srdf"
    #    SRDF_SUBPATH = "/tiago_description/srdf/" + SRDF_FILENAME
86
87
88
    URDF_SUBPATH = "/tiago_description/robots/" + URDF_FILENAME
    modelPath = getModelPath(URDF_SUBPATH)
    # Load URDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
89
    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
90
    # Load SRDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
91
    #    readParamsFromSrdf(robot, modelPath+SRDF_SUBPATH, False)
92
93
94
95
96
    return robot


def loadTiagoNoHand():
    URDF_FILENAME = "tiago_no_hand.urdf"
Carlos Mastalli's avatar
Carlos Mastalli committed
97
98
    #    SRDF_FILENAME = "tiago.srdf"
    #    SRDF_SUBPATH = "/tiago_description/srdf/" + SRDF_FILENAME
99
100
101
    URDF_SUBPATH = "/tiago_description/robots/" + URDF_FILENAME
    modelPath = getModelPath(URDF_SUBPATH)
    # Load URDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
102
    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
103
    # Load SRDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
104
    #    readParamsFromSrdf(robot, modelPath+SRDF_SUBPATH, False)
105
    return robot
106

Carlos Mastalli's avatar
Carlos Mastalli committed
107

108
109
110
111
112
def loadICub(reduced=True):
    if reduced:
        URDF_FILENAME = "icub_reduced.urdf"
    else:
        URDF_FILENAME = "icub.urdf"
113
114
    SRDF_FILENAME = "icub.srdf"
    SRDF_SUBPATH = "/icub_description/srdf/" + SRDF_FILENAME
115
116
117
    URDF_SUBPATH = "/icub_description/robots/" + URDF_FILENAME
    modelPath = getModelPath(URDF_SUBPATH)
    # Load URDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
118
    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
119
    # Load SRDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
120
    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False)
121
122
    # Add the free-flyer joint limits
    addFreeFlyerJointLimits(robot)
123
    return robot