unittest_utils.py 4.09 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
11
12
13
14
15
def getModelPath(subpath):
    for path in ['..', '../..', '/opt/openrobots/share/example-robot-data']:
        if exists(join(path, subpath.strip('/'))):
            print "using %s as modelPath" % path
            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))
23
    try:
Guilhem Saurel's avatar
Guilhem Saurel committed
24
        pinocchio.loadReferenceConfigurations(rmodel, SRDF_PATH, verbose)
25
26
        robot.q0.flat[:] = \
            rmodel.referenceConfigurations["half_sitting"].copy()
27
    except:
28
29
        print "loadReferenceConfigurations did not work. Please check your \
            Pinocchio Version"
Carlos Mastalli's avatar
Carlos Mastalli committed
30

Guilhem Saurel's avatar
Guilhem Saurel committed
31
32
33
34
35
36
37
38
        try:
            pinocchio.getNeutralConfiguration(rmodel, SRDF_PATH, verbose)
            robot.q0.flat[:] = rmodel.neutralConfiguration.copy()
        except:
            robot.q0.flat[:] = pinocchio.neutral(rmodel)
    return


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

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

Guilhem Saurel's avatar
Guilhem Saurel committed
52

53
def loadTalos():
54
55
    URDF_FILENAME = "talos_reduced.urdf"
    SRDF_FILENAME = "talos.srdf"
56
57
    SRDF_SUBPATH = "/talos_data/srdf/" + SRDF_FILENAME
    URDF_SUBPATH = "/talos_data/robots/" + URDF_FILENAME
58
    modelPath = getModelPath(URDF_SUBPATH)
59
    # Load URDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
60
    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
61
    # Load SRDF file
Carlos Mastalli's avatar
Carlos Mastalli committed
62
63
    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False)
    assert ((robot.model.armature[:6] == 0.).all())
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
    return robot
78
79
80
81


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


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

Carlos Mastalli's avatar
Carlos Mastalli committed
105

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