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