test_load.py 3.46 KB
Newer Older
Guilhem Saurel's avatar
Guilhem Saurel committed
1
#!/usr/bin/env python
Guilhem Saurel's avatar
Guilhem Saurel committed
2

3
import unittest
Guilhem Saurel's avatar
Guilhem Saurel committed
4

5
6
7
try:
    import pybullet
except ImportError:
8
    pybullet = False
9
10

from example_robot_data import load_full
Guilhem Saurel's avatar
Guilhem Saurel committed
11

12
13

class RobotTestCase(unittest.TestCase):
14
    def check(self, name, expected_nq, expected_nv, one_kg_bodies=[]):
Guilhem Saurel's avatar
Guilhem Saurel committed
15
        """Helper function for the real tests"""
16
        robot, _, urdf, _ = load_full(name, display=False)
Guilhem Saurel's avatar
Guilhem Saurel committed
17
18
19
        self.assertEqual(robot.model.nq, expected_nq)
        self.assertEqual(robot.model.nv, expected_nv)
        self.assertTrue(hasattr(robot, "q0"))
20
21
22
23
24
25
26
27
28
29
30
31
32
        if pybullet:
            self.check_pybullet(urdf, one_kg_bodies)

    def check_pybullet(self, urdf, one_kg_bodies):
        client_id = pybullet.connect(pybullet.DIRECT)
        robot_id = pybullet.loadURDF(urdf, physicsClientId=client_id)
        for joint_id in range(pybullet.getNumJoints(robot_id, client_id)):
            dynamics = pybullet.getDynamicsInfo(robot_id, joint_id, client_id)
            if dynamics[0] == 1:
                joint = pybullet.getJointInfo(robot_id, joint_id, client_id)
                # with self.subTest():  # uncomment on python >= 3.4 to get full list of wrong bodies at once
                self.assertIn(joint[12].decode(), one_kg_bodies)
        pybullet.disconnect(client_id)
33

Guilhem Saurel's avatar
Guilhem Saurel committed
34
    def test_anymal(self):
Guilhem Saurel's avatar
Guilhem Saurel committed
35
        self.check('anymal', 19, 18)
36

Guilhem Saurel's avatar
Guilhem Saurel committed
37
    def test_anymal_kinova(self):
Guilhem Saurel's avatar
Guilhem Saurel committed
38
        self.check('anymal_kinova', 25, 24)
39

40
41
    def test_double_pendulum(self):
        self.check('double_pendulum', 2, 2)
Guilhem Saurel's avatar
Guilhem Saurel committed
42

Guilhem Saurel's avatar
Guilhem Saurel committed
43
44
    def test_hector(self):
        self.check('hector', 7, 6)
45

46
47
    def test_hyq(self):
        self.check('hyq', 19, 18)
48

Guilhem Saurel's avatar
Guilhem Saurel committed
49
    def test_icub(self):
Guilhem Saurel's avatar
Guilhem Saurel committed
50
51
52
53
        self.check('icub', 39, 38)

    def test_icub_reduced(self):
        self.check('icub_reduced', 36, 35)
54

55
56
57
58
59
60
61
62
63
64
65
66
    def test_iris(self):
        self.check('iris', 7, 6)

    def test_kinova(self):
        self.check('kinova', 9, 6)

    def test_panda(self):
        self.check('panda', 9, 9)

    def test_romeo(self):
        self.check('romeo', 62, 61)

67
68
69
70
71
72
    def test_simple_humanoid(self):
        self.check('simple_humanoid', 36, 35, one_kg_bodies=['LARM_LINK3', 'RARM_LINK3'])

    def test_simple_humanoid_classical(self):
        self.check('simple_humanoid_classical', 36, 35, one_kg_bodies=['LARM_LINK3', 'RARM_LINK3'])

Guilhem Saurel's avatar
Guilhem Saurel committed
73
    def test_solo(self):
Guilhem Saurel's avatar
Guilhem Saurel committed
74
        self.check('solo', 15, 14)
75

Guilhem Saurel's avatar
Guilhem Saurel committed
76
    def test_solo12(self):
Guilhem Saurel's avatar
Guilhem Saurel committed
77
        self.check('solo12', 19, 18)
78

79
80
81
    def test_talos(self):
        self.check('talos', 39, 38)

82
83
84
    def test_talos_box(self):
        self.check('talos_box', 39, 38)

Guilhem Saurel's avatar
Guilhem Saurel committed
85
86
87
    def test_talos_full(self):
        self.check('talos_full', 51, 50)

88
89
90
    def test_talos_full_box(self):
        self.check('talos_full_box', 51, 50)

91
92
93
94
95
96
    def test_talos_arm(self):
        self.check('talos_arm', 7, 7)

    def test_talos_legs(self):
        self.check('talos_legs', 19, 18)

Guilhem Saurel's avatar
Guilhem Saurel committed
97
    def test_tiago(self):
Guilhem Saurel's avatar
Guilhem Saurel committed
98
        self.check('tiago', 50, 48)
99

Guilhem Saurel's avatar
Guilhem Saurel committed
100
    def test_tiago_no_hand(self):
Guilhem Saurel's avatar
Guilhem Saurel committed
101
        self.check('tiago_no_hand', 14, 12)
102

103
104
105
106
107
108
109
110
111
    def test_ur3(self):
        self.check('ur3', 6, 6)

    def test_ur3_gripper(self):
        self.check('ur3_gripper', 6, 6)

    def test_ur3_limited(self):
        self.check('ur3_limited', 6, 6)

Guilhem Saurel's avatar
Guilhem Saurel committed
112
    def test_ur5(self):
Guilhem Saurel's avatar
Guilhem Saurel committed
113
        self.check('ur5', 6, 6)
114

Guilhem Saurel's avatar
Guilhem Saurel committed
115
    def test_ur5_gripper(self):
Guilhem Saurel's avatar
Guilhem Saurel committed
116
        self.check('ur5_gripper', 6, 6)
Guilhem Saurel's avatar
Guilhem Saurel committed
117

118
119
    def test_ur5_limited(self):
        self.check('ur5_limited', 6, 6)
Guilhem Saurel's avatar
Guilhem Saurel committed
120

121
122
123
124
125
126
    def test_ur10(self):
        self.check('ur10', 6, 6)

    def test_ur10_limited(self):
        self.check('ur10_limited', 6, 6)

Guilhem Saurel's avatar
Guilhem Saurel committed
127

128
if __name__ == '__main__':
Guilhem Saurel's avatar
Guilhem Saurel committed
129
    unittest.main()