unittest_utils.py 2.47 KB
Newer Older
Guilhem Saurel's avatar
Guilhem Saurel committed
1
2
import numpy as np

3
4
5
6
7
import pinocchio
from pinocchio.robot_wrapper import RobotWrapper


def readParamsFromSrdf(robot, SRDF_PATH, verbose):
Guilhem Saurel's avatar
Guilhem Saurel committed
8
9
10
    rmodel = robot.model

    pinocchio.loadRotorParameters(rmodel, SRDF_PATH, verbose)
11
12
13
    rmodel.armature = \
        np.multiply(rmodel.rotorInertia.flat,
                    np.square(rmodel.rotorGearRatio.flat))
14
    try:
Guilhem Saurel's avatar
Guilhem Saurel committed
15
        pinocchio.loadReferenceConfigurations(rmodel, SRDF_PATH, verbose)
16
17
        robot.q0.flat[:] = \
            rmodel.referenceConfigurations["half_sitting"].copy()
18
    except:
19
20
        print "loadReferenceConfigurations did not work. Please check your \
            Pinocchio Version"
Guilhem Saurel's avatar
Guilhem Saurel committed
21
22
23
24
25
26
27
28
        try:
            pinocchio.getNeutralConfiguration(rmodel, SRDF_PATH, verbose)
            robot.q0.flat[:] = rmodel.neutralConfiguration.copy()
        except:
            robot.q0.flat[:] = pinocchio.neutral(rmodel)
    return


29
30
def loadTalosArm(modelPath='/opt/openrobots/share/example-robot-data'):
    URDF_FILENAME = "talos_left_arm.urdf"
31
    URDF_SUBPATH = "/talos_data/robots/" + URDF_FILENAME
32
    SRDF_FILENAME = "talos.srdf"
33
    SRDF_SUBPATH = "/talos_data/srdf/" + SRDF_FILENAME
34
    # Load URDF file
35
    robot = RobotWrapper.BuildFromURDF(modelPath+URDF_SUBPATH, [modelPath])
36
37

    # Load SRDF file
38
    readParamsFromSrdf(robot, modelPath+SRDF_SUBPATH, False)
39
40
    return robot

Guilhem Saurel's avatar
Guilhem Saurel committed
41

42
43
44
def loadTalos(modelPath='/opt/openrobots/share/example-robot-data'):
    URDF_FILENAME = "talos_reduced.urdf"
    SRDF_FILENAME = "talos.srdf"
45
46
    SRDF_SUBPATH = "/talos_data/srdf/" + SRDF_FILENAME
    URDF_SUBPATH = "/talos_data/robots/" + URDF_FILENAME
47
    # Load URDF file
48
    robot = RobotWrapper.BuildFromURDF(modelPath+URDF_SUBPATH, [modelPath],
49
                                       pinocchio.JointModelFreeFlyer())
50
    # Load SRDF file
51
    readParamsFromSrdf(robot, modelPath+SRDF_SUBPATH, False)
52
    assert((robot.model.armature[:6] == 0.).all())
53
54
55
56
57
    return robot


def loadHyQ(modelPath='/opt/openrobots/share/example-robot-data'):
    URDF_FILENAME = "hyq_no_sensors.urdf"
58
    URDF_SUBPATH = "/hyq_description/robots/" + URDF_FILENAME
59
60
    robot = RobotWrapper.BuildFromURDF(modelPath+URDF_SUBPATH, [modelPath],
                                       pinocchio.JointModelFreeFlyer())
61
    # TODO define default position inside srdf
62
63
    robot.q0.flat[7:] = [-0.2, 0.75, -1.5, -0.2, -
                         0.75, 1.5, -0.2, 0.75, -1.5, -0.2, -0.75, 1.5]
64
65
66
    robot.q0[2] = 0.57750958
    robot.model.referenceConfigurations["half_sitting"] = robot.q0
    return robot