1 /* Siconos is a program dedicated to modeling, simulation and control
2 * of non smooth dynamical systems.
3 *
4 * Copyright 2021 INRIA.
5 *
6 * Licensed under the Apache License, Version 2.0 (the "License");
7 * you may not use this file except in compliance with the License.
8 * You may obtain a copy of the License at
9 *
10 * http://www.apache.org/licenses/LICENSE-2.0
11 *
12 * Unless required by applicable law or agreed to in writing, software
13 * distributed under the License is distributed on an "AS IS" BASIS,
14 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15 * See the License for the specific language governing permissions and
16 * limitations under the License.
17 */
18
19 /* Simple twisting example with the double integrator */
20
21 /* to have drand48 ...
22 * hmhmhm ??? -- xhub */
23 #define _XOPEN_SOURCE
24
25 #include <stdio.h>
26 #include <stdlib.h>
27 #include <float.h>
28
29 #include "AVI_Solvers.h"
30 #include "AVI_cst.h"
31 #include "SiconosSets.h"
32 #include "NumericsMatrix.h"
33 #define TS 10e-3
34 #define NB_ITER 10000
35 #define TOL_NC 1e-12
36
main(void)37 int main(void)
38 {
39 double x[2];
40 unsigned short xsubi1[] = {0, 0, 0};
41 unsigned short xsubi2[] = {0, 0, 0};
42 x[0] = 50*erand48(xsubi1);
43 x[1] = 50*erand48(xsubi2);
44 /* column major */
45 double Hdat[8] = {1.0, -TS/2.0, -1.0, TS/2.0, 0.0, 1.0, 0.0, -1.0};
46 double K[4] = {-1.0, -1.0, -1.0, -1.0};
47
48 NumericsMatrix H;
49 NM_null(&H);
50 NM_fill(&H, NM_DENSE, 4, 2, Hdat);
51
52 double v1[] = {-1.0, -1.0 -TS/2.0};
53 double v2[] = {-1.0, 1.0 -TS/2.0};
54 double v3[] = {1.0, 1.0 + TS/2.0};
55 double v4[] = {1.0, -1.0 + TS/2.0};
56
57 polyhedron poly = { SICONOS_SET_POLYHEDRON, 4, 0, &H, K, NULL, NULL };
58
59 /* twisting gain */
60 double G = 10;
61 double beta = .1;
62 NumericsMatrix num_mat;
63 double M[4] = { G*TS*TS/2.0, G*TS, beta*G*TS*TS/2.0, beta*G*TS };
64 NM_null(&num_mat);
65 NM_fill(&num_mat, NM_DENSE, 2, 2, M);
66
67 double q[2] = { x[0] + TS*x[1], x[1] };
68
69 AffineVariationalInequalities avi;
70 avi.size = 2;
71 avi.M = &num_mat;
72 avi.q = q;
73 avi.d = NULL;
74 avi.poly.split = &poly;
75
76 SolverOptions * options = solver_options_create(SICONOS_AVI_CAOFERRIS);
77
78 _Bool c1, c2, c3, c4;
79 unsigned N = 0;
80 int info = 0;
81 double lambda[2] = {0.};
82 double sigma[2] = {0.};
83 do
84 {
85 N++;
86 lambda[0] = 0.0;
87 lambda[1] = 0.0;
88 sigma[0] = 0.0;
89 sigma[1] = 0.0;
90
91 info = avi_caoferris(&avi, lambda, sigma, options);
92 if(info) fprintf(stderr, "SOLVER FAILED!\tinfo=%d\n", info);
93
94 /* / printf("x_k: %2.6e %2.6e\n", x[0], x[1]);
95 printf("lambda: %2.6e %2.6e\n", lambda[0], lambda[1]);
96 printf("u = %2.6e\n", lambda[0] + beta*lambda[1]);*/
97 sigma[0] = x[0] + TS*x[1] + M[0]*lambda[0] + M[2]*lambda[1];
98 sigma[1] = x[1] + M[1]*lambda[0] + M[3]*lambda[1];
99 /* printf("x_{k+1}: %2.6e %2.6e\n", sigma[0], sigma[1]);*/
100
101 /* let's check is it is correct */
102 c1 = (sigma[0]*(v1[0] + lambda[0]) + sigma[1]*(v1[1] + lambda[1])) <= TOL_NC;
103 c2 = (sigma[0]*(v2[0] + lambda[0]) + sigma[1]*(v2[1] + lambda[1])) <= TOL_NC;
104 c3 = (sigma[0]*(v3[0] + lambda[0]) + sigma[1]*(v3[1] + lambda[1])) <= TOL_NC;
105 c4 = (sigma[0]*(v4[0] + lambda[0]) + sigma[1]*(v4[1] + lambda[1])) <= TOL_NC;
106
107 if(!c1)
108 {
109 fprintf(stderr, "ERROR in implicit_twisting.c, bad value of lambda. test c1 failed\n");
110 fprintf(stderr, "<v1-lambda, sigma> = %2.10e\n", (sigma[0]*(v1[0] + lambda[0]) + sigma[1]*(v1[1] + lambda[1])));
111 goto expose_failure;
112 }
113 if(!c2)
114 {
115 fprintf(stderr, "ERROR in implicit_twisting.c, bad value of lambda. test c2 failed\n");
116 fprintf(stderr, "<v2-lambda, sigma> = %2.10e\n", (sigma[0]*(v2[0] + lambda[0]) + sigma[1]*(v2[1] + lambda[1])));
117 goto expose_failure;
118 }
119
120 if(!c3)
121 {
122 fprintf(stderr, "ERROR in implicit_twisting.c, bad value of lambda. test c3 failed\n");
123 fprintf(stderr, "<v3-lambda, sigma> = %2.10e\n", (sigma[0]*(v3[0] + lambda[0]) + sigma[1]*(v3[1] + lambda[1])));
124 goto expose_failure;
125 }
126
127 if(!c4)
128 {
129 fprintf(stderr, "ERROR in implicit_twisting.c, bad value of lambda. test c4 failed\n");
130 fprintf(stderr, "<v4-lambda, sigma> = %2.10e\n", (sigma[0]*(v4[0] + lambda[0]) + sigma[1]*(v4[1] + lambda[1])));
131 goto expose_failure;
132 }
133 x[0] = sigma[0];
134 x[1] = sigma[1];
135 q[0] = x[0] + TS*x[1];
136 q[1] = x[1];
137 }
138 while(!info && c1 && c2 && c3 && c4 && (N <= NB_ITER));
139
140 printf("final x: %2.6e %2.6e\n", sigma[0], sigma[1]);
141
142 solver_options_delete(options);
143 return info;
144
145 expose_failure:
146 fprintf(stderr, "x = %2.10e %2.10e\n", x[0], x[1]);
147 fprintf(stderr, "q = %2.10e %2.10e\n", q[0], q[1]);
148 fprintf(stderr, "lambda = %2.10e %2.10e\n", lambda[0], lambda[1]);
149
150 return 1;
151 }
152