1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
|
import platform
import pytest
import math
import numpy as np
import dartpy as dart
def test_inertia_init():
'''
Test basic functionality for the `dartpy.dynamics.Inertia` class.
'''
# test default values
i1 = dart.dynamics.Inertia()
assert i1 is not None
# initialize with parameters
i2 = dart.dynamics.Inertia(0.1, [0, 0, 0], 1.3*np.eye(3))
assert i1 is not None
newMass = 1.5
i2.setMass(newMass)
assert i2.getMass() == newMass
newCOM = np.array((0.1, 0, 0))
i2.setLocalCOM(newCOM)
assert np.allclose(i2.getLocalCOM(), newCOM)
newMoment = 0.4*newMass*0.1**2*np.eye(3)
i2.setMoment(newMoment)
assert np.allclose(i2.getMoment(), newMoment)
i2.setSpatialTensor(0.3*i2.getSpatialTensor())
assert i2.verify()
for i in range(10): # based on the C++ tests
mass = np.random.uniform(0.1, 10.0)
com = np.random.uniform(-5, 5, 3)
I = np.random.rand(3, 3) - 0.5 + \
np.diag(np.random.uniform(0.6, 1, 3), 0)
I = (I + I.T)/2
inertia = dart.dynamics.Inertia(mass, com, I)
assert inertia.verify()
def test_inertia_static_methods():
'''
Test the class methods `verifyMoment`and `verifySpatialTensor`.
'''
assert dart.dynamics.Inertia.verifyMoment(np.eye(3), printWarnings=False)
for i in range(10):
I = np.random.rand(3, 3) - 0.5 + \
np.diag(np.random.uniform(1, 10, 3), 0)
I = (I + I.T)/2
assert dart.dynamics.Inertia.verifyMoment(I)
assert dart.dynamics.Inertia.verifySpatialTensor(
np.eye(6), printWarnings=False)
def test_failing_moment_and_spatial():
'''
Test some failure cases of the verify methods.
'''
for i in range(10):
I = np.random.rand(3, 3) - 0.5 - \
np.diag(np.random.uniform(1, 10, 3), 0)
assert not dart.dynamics.Inertia.verifyMoment(I, printWarnings=False)
# fails e.g. due to off diagonal values in translational part.
assert not dart.dynamics.Inertia.verifySpatialTensor(
np.random.rand(6, 6), printWarnings=False)
if __name__ == "__main__":
pytest.main()
|
