1# Copyright 2021 INRIA
2#!/usr/bin/env python
3
4import numpy as np
5
6import siconos.numerics as sn
7
8
9def vi_function_1D(n, x, F):
10    F[0] = 1.0 + x[0]
11    pass
12
13
14def vi_nabla_function_1D(n, x, nabla_F):
15    nabla_F[0] = 1.0
16    pass
17
18
19def vi_function_2D(n, z, F):
20    M = np.array([[2., 1.],
21                  [1., 2.]])
22
23    q = np.array([-5., -6.])
24    F[:] = np.dot(M, z) + q
25    pass
26
27
28def vi_nabla_function_2D(n, z, nabla_F):
29    M = np.array([[2., 1.],
30                  [1., 2.]])
31    nabla_F[:] = M
32    pass
33
34
35def vi_function_3D(n, z, F):
36    M = np.array(((0., -1., 2.),
37                  (2., 0., -2.),
38                  (-1., 1., 0.)))
39
40    q = np.array((-3., 6., -1))
41    F[:] = np.dot(M, z) + q
42    pass
43
44
45def vi_nabla_function_3D(n, z, nabla_F):
46    M = np.array(((0., -1., 2.),
47                  (2., 0., -2.),
48                  (-1., 1., 0.)))
49    nabla_F[:] = M
50    pass
51
52
53# solution
54xsol_1D = np.array([-1.0])
55Fsol_1D = np.array([0.0])
56
57xsol_2D = np.array([1.0, 1.0])
58Fsol_2D = np.array([-2.0, -3.0])
59
60xsol_3D = np.array((-1.0, -1.0, 1.0))
61Fsol_3D = np.array((0., 2.0, -1.0))
62# problem
63#vi=N.MCP(1,1,vi_function,vi_Nablafunction)
64
65xtol = 1e-8
66
67
68def test_new():
69    return sn.VI(1)
70
71
72def test_vi_1D():
73    vi = sn.VI(1, vi_function_1D)
74    vi.set_compute_nabla_F(vi_nabla_function_1D)
75    x = np.array([0.])
76    F = np.array([0.])
77
78    SO = sn.SolverOptions(sn.SICONOS_VI_BOX_QI)
79    lb = np.array((-1.0,))
80    ub = np.array((1.0,))
81    vi.set_box_constraints(lb, ub)
82    info = sn.variationalInequality_box_newton_QiLSA(vi, x, F, SO)
83    print(info)
84    print("x = ", x)
85    print("F = ", F)
86    assert (np.linalg.norm(x - xsol_1D) <= xtol)
87    assert not info
88
89
90def test_vi_2D():
91    vi = sn.VI(2, vi_function_2D)
92    vi.set_compute_nabla_F(vi_nabla_function_2D)
93    x = np.array((0., 0.))
94    F = np.array((0., 0.))
95
96    SO = sn.SolverOptions(sn.SICONOS_VI_BOX_QI)
97    lb = np.array((-1.0, -1.0))
98    ub = np.array((1.0, 1.0))
99    vi.set_box_constraints(lb, ub)
100    info = sn.variationalInequality_box_newton_QiLSA(vi, x, F, SO)
101    print(info)
102    print('number of iteration {:} ; precision {:}'.format(
103        SO.iparam[sn.SICONOS_IPARAM_ITER_DONE],
104        SO.dparam[sn.SICONOS_DPARAM_RESIDU]))
105    print("x = ", x)
106    print("F = ", F)
107    assert (np.linalg.norm(x - xsol_2D) <= xtol)
108    assert not info
109
110
111def test_vi_3D():
112    vi = sn.VI(3, vi_function_3D)
113    x = np.zeros((3,))
114    F = np.zeros((3,))
115
116    SO = sn.SolverOptions(sn.SICONOS_VI_BOX_QI)
117    vi.set_compute_nabla_F(vi_nabla_function_3D)
118    lb = np.array((-1.0, -1.0, -1.0))
119    ub = np.array((1.0, 1.0, 1.0))
120    vi.set_box_constraints(lb, ub)
121    info = sn.variationalInequality_box_newton_QiLSA(vi, x, F, SO)
122    print(info)
123    print('number of iteration {:} ; precision {:}'.format(
124        SO.iparam[sn.SICONOS_IPARAM_ITER_DONE],
125        SO.dparam[sn.SICONOS_DPARAM_RESIDU]))
126    print("x = ", x)
127    print("F = ", F)
128    assert (np.linalg.norm(x - xsol_3D) <= xtol)
129    assert not info
130    assert(np.abs(SO.dparam[sn.SICONOS_DPARAM_RESIDU]) < 1e-10)
131
132
133def test_vi_C_interface():
134    try:
135        from cffi import FFI
136        cffi_is_present = True
137    except:
138        cffi_is_present = False
139        return
140
141    if cffi_is_present:
142        h = 1e-5
143        T = 1.0
144        t = 0.0
145
146        theta = 1.0
147        gamma = 1.0
148        g = 9.81
149        kappa = 0.4
150
151        xk = np.array((1., 10.))
152        ffi = FFI()
153        ffi.cdef('void set_cstruct(uintptr_t p_env, void* p_struct);')
154        ffi.cdef('''typedef struct
155                 {
156                 int id;
157                 double* xk;
158                 double h;
159                 double theta;
160                 double gamma;
161                 double g;
162                 double kappa;
163                 unsigned int f_eval;
164                 unsigned int nabla_eval;
165                  } data;
166                 ''')
167
168        data_struct = ffi.new('data*')
169        data_struct.id = -1  # to avoid freeing the data in the destructor
170        data_struct.xk = ffi.cast('double *', xk.ctypes.data)
171        data_struct.h = h
172        data_struct.theta = theta
173        data_struct.gamma = gamma
174        data_struct.g = g
175        data_struct.kappa = kappa
176
177        vi = sn.VI(2)
178        import siconos
179        D = ffi.dlopen(siconos.__path__[0] + '/_numerics.so')
180        D.set_cstruct(vi.get_env_as_long(), ffi.cast('void*', data_struct))
181        vi.set_compute_F_and_nabla_F_as_C_functions('ZhuravlevIvanov.so', 'compute_F', 'compute_nabla_F')
182
183        lambda_ = np.zeros((2,))
184        xkp1 = np.zeros((2,))
185
186        SO = sn.SolverOptions(sn.SICONOS_VI_BOX_QI)
187        lb = np.array((-1.0, -1.0))
188        ub = np.array((1.0, 1.0))
189        vi.set_box_constraints(lb, ub)
190
191        N = int(T / h + 10)
192        print(N)
193        SO.dparam[sn.SICONOS_DPARAM_TOL] = 1e-24
194        SO.iparam[sn.SICONOS_IPARAM_MAX_ITER] = 100
195        SO.iparam[sn.SICONOS_VI_IPARAM_ACTIVATE_UPDATE] = 1
196        SO.iparam[sn.SICONOS_VI_IPARAM_DECREASE_RHO] = 0
197        SO.iparam[4] = 5 # ???
198
199        signs = np.empty((N, 2))
200        sol = np.empty((N, 2))
201        sol[0, :] = xk
202
203        k = 0
204        #sn.numerics_set_verbose(3)
205
206        while t <= T:
207            k += 1
208            info = sn.variationalInequality_box_newton_QiLSA(vi, lambda_, xkp1, SO)
209            if info > 0:
210                print(lambda_)
211    #            vi_function(2, signs[k-1, :], xkp1)
212                lambda_[0] = -np.sign(xkp1[0])
213                lambda_[1] = -np.sign(xkp1[1])
214                if np.abs(xk[0]) < 1e-10:
215                    lambda_[0] = 0.01
216                if np.abs(xk[1]) < 1e-10:
217                    lambda_[1] = 0.01
218                    print('ok lambda')
219                    print(lambda_)
220                info = sn.variationalInequality_box_newton_QiLSA(vi, lambda_, xkp1, SO)
221                print('iter {:} ; solver iter = {:} ; prec = {:}'.format(
222                    k, SO.iparam[sn.SICONOS_IPARAM_ITER_DONE],
223                    SO.dparam[sn.SICONOS_DPARAM_RESIDU]))
224                if info >0:
225                    print('VI solver failed ! info = {:}'.format(info))
226                    print(xk)
227                    print(lambda_)
228                    print(xkp1)
229                    kaboom()
230            sol[k, 0:2] = xkp1
231            np.copyto(xk, xkp1, casting='no')
232            signs[k, 0:2] = lambda_
233            t = k*h
234            #z[:] = 0.0
235
236