1#!/usr/bin/env python
2
3import numpy as np
4from siconos.tests_setup import working_dir
5import os
6import siconos.kernel as sk
7
8ndof = 3
9nn = 2 * ndof
10x0 = np.zeros(nn, dtype=np.float64)
11q0 = np.zeros(ndof, dtype=np.float64)
12v0 = np.zeros(ndof, dtype=np.float64)
13q_ne = np.zeros(nn + 1, dtype=np.float64)
14inertia = np.eye(ndof, dtype=np.float64)
15a_mat = np.asarray(np.random.random((nn, nn)), dtype=np.float64)
16
17# List of all existing DS classes and standard initialisation paramaters
18ds_classes = {
19    sk.FirstOrderNonLinearDS: (x0,),
20    sk.FirstOrderLinearDS: (x0, a_mat),
21    sk.FirstOrderLinearTIDS: (x0, a_mat),
22    sk.LagrangianDS: (q0, v0, inertia),
23    sk.LagrangianLinearTIDS: (q0, v0, inertia),
24    sk.NewtonEulerDS: (q_ne, x0, 1., inertia)}
25
26
27def test_ds_interface():
28    """Tests methods that should be available
29    for all dynamical systems
30    """
31    time = 1.
32
33    for args in ds_classes:
34        class_name = args
35        attr = ds_classes[args]
36        ds = class_name(*attr)
37        print(class_name, attr[0])
38        ds.initializeNonSmoothInput(1)
39        ds.resetAllNonSmoothParts()
40        ds.resetNonSmoothPart(1)
41        ds.initRhs(time)
42        ds.computeRhs(time)
43        assert ds.number() >= 0
44        assert ds.rhs() is not None
45        ds.update(time)
46        ds.resetToInitialState()
47        ds.display()
48        ds.computeJacobianRhsx(time)
49        assert ds.jacobianRhsx() is not None
50
51
52def test_first_order_nlds():
53    """Build and test first order non linear ds
54    """
55    ndof = 3
56    x0 = np.zeros(ndof, dtype=np.float64)
57    ds = sk.FirstOrderNonLinearDS(x0)
58    ds.display()
59    time = 1.2
60    ds.computeRhs(time)
61    ds.computeJacobianRhsx(time)
62    assert ds.dimension() == ndof
63    assert not ds.isLinear()
64    assert np.allclose(ds.x0(), x0)
65    assert np.allclose(ds.x(), x0)
66    assert np.allclose(ds.rhs(), 0.)
67    ds.computef(time, ds.x())
68    assert ds.f() is None
69    ds.initRhs(time)
70    assert ds.jacobianfx() is None
71    assert np.allclose(ds.jacobianRhsx(), 0.)
72
73
74def test_first_order_lds():
75    """Build and test first order linear
76    and time-invariant coeff. ds
77    """
78    ndof = 3
79    x0 = np.random.random(ndof)
80    time = 1.2
81    ds_list = []
82    a_mat = np.random.random((ndof, ndof))
83    b_vec = np.random.random((ndof, ))
84    ds_list.append(sk.FirstOrderLinearDS(x0))
85    ds_list.append(sk.FirstOrderLinearDS(x0, a_mat))
86    ds_list.append(sk.FirstOrderLinearDS(x0, a_mat, b_vec))
87
88    for ds in ds_list:
89        assert ds.isLinear()
90        assert ds.dimension() == ndof
91        assert np.allclose(ds.x0(), x0)
92        assert np.allclose(ds.x(), x0)
93        assert np.allclose(ds.r(), 0.)
94
95        rhs = np.zeros_like(ds.x())
96        jac_ref = np.zeros((ndof, ndof), dtype=np.float64)
97        if isinstance(ds.A(), np.ndarray):
98            jac_ref += a_mat
99            rhs += np.dot(a_mat, ds.x())
100        if isinstance(ds.b(), np.ndarray):
101            rhs += ds.b()
102        ds.computef(time, ds.x())
103        if ds.f() is not None:
104            assert np.allclose(rhs, ds.f())
105
106        ds.initRhs(time)
107        assert np.allclose(rhs, ds.rhs())
108        if ds.A() is not None:
109            assert np.allclose(ds.jacobianRhsx(), jac_ref)
110            assert np.allclose(ds.jacobianRhsx(), ds.jacobianfx())
111
112
113def test_first_order_ltids():
114    """Build and test first order linear
115    and time-invariant coeff. ds
116    """
117    time = 1.2
118    ds_list = []
119    b_vec = np.random.random((nn, ))
120    ds_list.append(sk.FirstOrderLinearTIDS(x0, a_mat))
121    ds_list.append(sk.FirstOrderLinearTIDS(x0, a_mat, b_vec))
122
123    for ds in ds_list:
124        assert ds.isLinear()
125        assert ds.dimension() == nn
126        assert np.allclose(ds.x0(), x0)
127        assert np.allclose(ds.x(), x0)
128        assert np.allclose(ds.r(), 0.)
129
130        rhs = np.dot(a_mat, ds.x())
131        if ds.b() is not None:
132            rhs += ds.b()
133        ds.initRhs(time)
134        assert np.allclose(rhs, ds.rhs())
135        assert np.allclose(ds.jacobianRhsx(), a_mat)
136        assert np.allclose(ds.jacobianRhsx(), ds.jacobianfx())
137
138
139def test_lagrangian_ds():
140    """Build and test lagrangian ds
141    """
142    q0[...] = [1, 2, 3]
143    v0[...] = [4, 5, 6]
144
145    mass = np.asarray(np.diag([1, 2, 3]), dtype=np.float64)
146    ds = sk.LagrangianDS(q0, v0, mass)
147    ec = ds.computeKineticEnergy()
148    assert ec == 87.
149    assert ds.dimension() == ndof
150    assert np.allclose(ds.mass(), mass)
151
152
153def test_lagrangian_tids():
154    """Build and test lagrangian linear and time-invariant ds
155    """
156    q0[...] = [1, 2, 3]
157    v0[...] = [4, 5, 6]
158
159    mass = np.asarray(np.diag([1, 2, 3]), dtype=np.float64)
160    stiffness = np.zeros((ndof, ndof), dtype=np.float64)
161    stiffness.flat[...] = np.arange(9)
162    damping = np.zeros_like(stiffness)
163    damping.flat[...] = np.arange(9, 18)
164    ds = sk.LagrangianLinearTIDS(q0, v0, mass, stiffness, damping)
165    ec = ds.computeKineticEnergy()
166    assert ec == 87.
167    assert ds.dimension() == ndof
168    assert np.allclose(ds.mass(), mass)
169    assert np.allclose(ds.K(), stiffness)
170    assert np.allclose(ds.C(), damping)
171    q = ds.q()
172    v = ds.velocity()
173    fref = -np.dot(stiffness, q)
174    fref -= np.dot(damping, v)
175    time = 0.3
176    ds.computeForces(time, q, v)
177    assert np.allclose(fref, ds.forces())
178    ds.computeJacobianqForces(time)
179    assert np.allclose(stiffness, ds.jacobianqForces())
180    ds.computeJacobianvForces(time)
181    assert np.allclose(damping, ds.jacobianvForces())
182
183
184if __name__ == "__main__":
185    # execute only if run as a script
186    test_lagrangian_tids()
187