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 #include <assert.h> // for assert
19 #include <math.h> // for pow, NAN
20 #include <stdio.h> // for printf, NULL
21 #include <stdlib.h> // for calloc, free, malloc
22 #include "NumericsFwd.h" // for SolverOptions, Varia...
23 #include "SolverOptions.h" // for SolverOptions, solve...
24 #include "VI_cst.h" // for SICONOS_VI_HP
25 #include "VariationalInequality.h" // for VariationalInequality
26 #include "VariationalInequality_Solvers.h" // for variationalInequalit...
27 #include "VariationalInequality_computeError.h" // for variationalInequalit...
28 #include "siconos_debug.h" // for DEBUG_PRINTF, DEBUG_...
29 #include "numerics_verbose.h" // for verbose
30 #include "SiconosBlas.h" // for cblas_dcopy, cblas_d...
31
variationalInequality_HyperplaneProjection(VariationalInequality * problem,double * x,double * w,int * info,SolverOptions * options)32 void variationalInequality_HyperplaneProjection(VariationalInequality* problem, double *x, double *w, int* info, SolverOptions* options)
33 {
34 /* /\* int and double parameters *\/ */
35 int* iparam = options->iparam;
36 double* dparam = options->dparam;
37 /* Number of contacts */
38 int n = problem->size;
39 /* Maximum number of iterations */
40 int itermax = iparam[SICONOS_IPARAM_MAX_ITER];
41 /* Maximum number of iterations in Line--search */
42 int lsitermax = iparam[SICONOS_VI_IPARAM_LS_MAX_ITER];
43 assert(lsitermax >0);
44 /* Tolerance */
45 double tolerance = dparam[SICONOS_DPARAM_TOL];
46
47
48 /***** Fixed point iterations *****/
49 int iter = 0; /* Current iteration number */
50 double error = 1.; /* Current error */
51 int hasNotConverged = 1;
52
53 double * xtmp = (double *)calloc(n, sizeof(double));
54 double * wtmp = (double *)calloc(n, sizeof(double));
55 double * xtmp2 = (double *)calloc(n, sizeof(double));
56 double * xtmp3 = (double *)calloc(n, sizeof(double));
57
58 int isVariable = 0;
59
60 double tau = 1.0;
61 double sigma = 0.99;
62
63 if(dparam[SICONOS_VI_DPARAM_LS_TAU] > 0.0)
64 {
65 tau = dparam[SICONOS_VI_DPARAM_LS_TAU];
66 }
67 else
68 {
69 printf("Hyperplane Projection method. tau <=0 is not well defined\n");
70 printf("Hyperplane Projection method. tau is set to 1.0\n");
71 }
72
73 if(dparam[SICONOS_VI_DPARAM_SIGMA] > 0.0 && dparam[SICONOS_VI_DPARAM_SIGMA] < 1.0)
74 {
75 sigma = dparam[SICONOS_VI_DPARAM_SIGMA];
76 }
77 else
78 {
79 printf("Hyperplane Projection method. 0<sigma <1 is not well defined\n");
80 printf("Hyperplane Projection method. sigma is set to %6.4e\n", sigma);
81 }
82
83
84 isVariable=0;
85
86
87 if(!isVariable)
88 {
89 /* double minusrho = -1.0*rho; */
90 while((iter < itermax) && (hasNotConverged > 0))
91 {
92 ++iter;
93 /** xtmp <-- x (x_k) */
94 cblas_dcopy(n, x, 1, xtmp, 1);
95
96 /* xtmp (y_k)= P_X(x_k-tau F(x_k)) */
97 problem->F(problem, n, xtmp, wtmp);
98 cblas_daxpy(n, -tau, wtmp, 1, xtmp, 1) ;
99 cblas_dcopy(n, xtmp, 1, xtmp2, 1);
100 problem->ProjectionOnX(problem, xtmp2,xtmp);
101
102 // Armijo line search
103
104 int stopingcriteria = 1;
105 int ls_iter = -1;
106 double alpha = 1.0;
107 double lhs = NAN;
108 double rhs;
109 // xtmp3 = z_k-y_k
110 cblas_dcopy(n, x, 1, xtmp3, 1);
111 cblas_daxpy(n, -1.0, xtmp, 1, xtmp3, 1);
112 rhs = cblas_dnrm2(n,xtmp3, 1);
113 rhs = sigma / tau * rhs * rhs;
114 DEBUG_EXPR_WE(
115 for(int i =0; i< n ; i++)
116 {
117 printf("(y_k) xtmp[%i]=%6.4e\t",i,xtmp[i]);
118 printf("(x_k-y_k) xtmp3[%i]=%6.4e\n",i,xtmp3[i]);
119 }
120 );
121 while(stopingcriteria && (ls_iter < lsitermax))
122 {
123 ls_iter++ ;
124 /* xtmp2 = alpha * y_k + (1-alpha) x_k */
125 alpha = 1.0 / (pow(2.0, ls_iter));
126 DEBUG_PRINTF("alpha = %6.4e\n", alpha);
127 cblas_dcopy(n,xtmp, 1, xtmp2, 1);
128 cblas_dscal(n, alpha, xtmp2, 1);
129 cblas_daxpy(n, 1.0-alpha, x, 1, xtmp2, 1);
130
131
132
133 /* wtmp = */
134
135 problem->F(problem, n, xtmp2,wtmp);
136 DEBUG_EXPR_WE(
137 for(int i =0; i< n ; i++)
138 {
139 printf("(z_k) xtmp2[%i]=%6.4e\n",i,xtmp2[i]);
140 printf("F(z_k) wtmp[%i]=%6.4e\n",i,wtmp[i]);
141 }
142 );
143 lhs = cblas_ddot(n, wtmp, 1, xtmp3, 1);
144
145 if(lhs >= rhs) stopingcriteria = 0;
146
147 DEBUG_PRINTF("ls_iter= %i, lsitermax =%i, stopingcriteria %i\n",ls_iter,lsitermax,stopingcriteria);
148 DEBUG_PRINTF("Number of iteration in Armijo line search = %i\t, lhs = %6.4e\t, rhs = %6.4e\t, alpha = %6.4e\t, sigma = %6.4e\t, tau = %6.4e\n", ls_iter, lhs, rhs, alpha, sigma, tau);
149
150 }
151
152
153
154 cblas_dcopy(n, x, 1, xtmp3, 1);
155 cblas_daxpy(n, -1.0, xtmp2, 1, xtmp3, 1);
156 DEBUG_PRINTF("norm(x-x_k) = %6.4e\n",cblas_dnrm2(n, xtmp3, 1));
157 lhs=cblas_ddot(n, wtmp, 1, xtmp3, 1);
158
159 double nonorm = cblas_dnrm2(n, wtmp, 1);
160 double rhoequiv = lhs / (nonorm * nonorm);
161
162 /* rhoequiv =1.0; */
163 DEBUG_PRINTF("nonorm = %6.4e\n", nonorm);
164 DEBUG_PRINTF("lhs = %6.4e\n", lhs);
165 DEBUG_PRINTF("rho equiv = %6.4e\n", rhoequiv);
166
167 cblas_daxpy(n, -rhoequiv, wtmp, 1, x, 1);
168
169 cblas_dcopy(n, x, 1, xtmp, 1);
170 problem->ProjectionOnX(problem, xtmp,x);
171
172
173
174 /* **** Criterium convergence **** */
175 variationalInequality_computeError(problem, x, w, tolerance, options, &error);
176 DEBUG_EXPR_WE(
177 for(int i =0; i< n ; i++)
178 {
179 printf("x[%i]=%6.4e\t",i,x[i]);
180 printf("w[%i]=F[%i]=%6.4e\n",i,i,w[i]);
181 }
182 );
183 if(options->callback)
184 {
185 options->callback->collectStatsIteration(options->callback->env, n,
186 x, w,
187 error, NULL);
188 }
189
190 if(verbose > 0)
191 {
192 printf("--------------- VI - Hyperplane Projection (HP) - Iteration %i tau = %14.7e \t rhoequiv = %14.7e \tError = %14.7e\n", iter, tau, rhoequiv, error);
193 }
194 if(error < tolerance) hasNotConverged = 0;
195 *info = hasNotConverged;
196 }
197 }
198
199
200
201 if(verbose > 0)
202 {
203 printf("--------------- VI - Hyperplane Projection (HP) - #Iteration %i Final Residual = %14.7e\n", iter, error);
204 }
205 dparam[SICONOS_DPARAM_RESIDU] = error;
206 iparam[SICONOS_IPARAM_ITER_DONE] = iter;
207 free(xtmp);
208 free(xtmp2);
209 free(xtmp3);
210 free(wtmp);
211
212 }
213
214
variationalInequality_HyperplaneProjection_set_default(SolverOptions * options)215 void variationalInequality_HyperplaneProjection_set_default(SolverOptions* options)
216 {
217 options->iparam[SICONOS_VI_IPARAM_LS_MAX_ITER] = 100;
218 options->dparam[SICONOS_VI_DPARAM_LS_TAU] = 1.0; /*tau */
219 options->dparam[SICONOS_VI_DPARAM_SIGMA] = 0.8; /* sigma */
220 }
221