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 <stdio.h> // for printf, fprintf, stderr
20 #include <stdlib.h> // for exit, EXIT_FAILURE
21 #include "LCP_Solvers.h" // for lcp_pivot, lcp_compute_error
22 #include "LinearComplementarityProblem.h" // for LinearComplementarityProblem
23 #include "NonSmoothDrivers.h" // for linearComplementarity_driver
24 #include "NumericsFwd.h" // for SolverOptions, LinearCompl...
25 #include "NumericsMatrix.h" // for NumericsMatrix
26 #include "SolverOptions.h" // for SolverOptions, solver_opti...
27 /* #define DEBUG_STDOUT */
28 /* #define DEBUG_MESSAGES */
29 #include "siconos_debug.h" // for DEBUG_END, DEBUG_BEGIN
30 #include "lcp_cst.h" // for SICONOS_LCP_IPARAM_PIVOTIN...
31 #include "numerics_verbose.h" // for numerics_error, verbose
32
33 const char* const SICONOS_LCP_LEMKE_STR = "Lemke";
34 const char* const SICONOS_LCP_NSGS_SBM_STR = "NSGS_SBM";
35 const char* const SICONOS_LCP_PGS_STR = "PGS";
36 const char* const SICONOS_LCP_CPG_STR = "CPG";
37 const char* const SICONOS_LCP_LATIN_STR = "Latin";
38 const char* const SICONOS_LCP_LATIN_W_STR = "Latin_w";
39 const char* const SICONOS_LCP_QP_STR = "QP";
40 const char* const SICONOS_LCP_NSQP_STR = "NSQP";
41 const char* const SICONOS_LCP_NEWTONMIN_STR = "NewtonMin";
42 const char* const SICONOS_LCP_NEWTON_FB_FBLSA_STR = "NewtonFB";
43 const char* const SICONOS_LCP_NEWTON_MIN_FBLSA_STR = "NewtonMinFB";
44 const char* const SICONOS_LCP_PSOR_STR = "PSOR";
45 const char* const SICONOS_LCP_RPGS_STR = "RPGS";
46 const char* const SICONOS_LCP_PATH_STR = "PATH";
47 const char* const SICONOS_LCP_ENUM_STR = "ENUM";
48 const char* const SICONOS_LCP_AVI_CAOFERRIS_STR = "AVI CaoFerris";
49 const char* const SICONOS_LCP_PIVOT_STR = "Pivot based method";
50 const char* const SICONOS_LCP_BARD_STR = "Bard-type pivoting method";
51 const char* const SICONOS_LCP_MURTY_STR = "Murty's least index pivoting method";
52 const char* const SICONOS_LCP_PATHSEARCH_STR = "For testing only: solver used in the Pathsearch algorithm";
53 const char* const SICONOS_LCP_PIVOT_LUMOD_STR = "Pivot based method with BLU updates using LUMOD";
54 const char* const SICONOS_LCP_GAMS_STR = "Using GAMS solvers";
55 const char* const SICONOS_LCP_CONVEXQP_PG_STR = "Convex QP Projected Gradient";
56
57 static int lcp_driver_SparseBlockMatrix(LinearComplementarityProblem* problem, double *z, double *w, SolverOptions* options);
58
lcp_driver_SparseBlockMatrix(LinearComplementarityProblem * problem,double * z,double * w,SolverOptions * options)59 int lcp_driver_SparseBlockMatrix(LinearComplementarityProblem* problem, double *z, double *w, SolverOptions* options)
60 {
61 DEBUG_BEGIN("lcp_driver_SparseBlockMatrix(...)\n");
62 /* Checks storage type for the matrix M of the LCP */
63 if(problem->M->storageType == 0)
64 numerics_error("lcp_driver_SparseBlockMatrix", "forbidden type of storage for the matrix M of the LCP");
65
66 /*
67 The options for the global "block" solver are defined in options->\n
68 options[i], for 0<i<numberOfSolvers-1 correspond to local solvers.
69 */
70
71 /* Output info. : 0: ok - >0: problem (depends on solver) */
72 int info = -1;
73
74 /******************************************
75 * 1 - Check for trivial solution
76 ******************************************/
77 int i = 0;
78 int n = problem->size;
79 double *q = problem->q;
80
81 if(options->iparam[SICONOS_LCP_IPARAM_SKIP_TRIVIAL] == SICONOS_LCP_SKIP_TRIVIAL_NO)
82 {
83 while((i < (n - 1)) && (q[i] >= 0.)) i++;
84 if((i == (n - 1)) && (q[n - 1] >= 0.))
85 {
86 /* TRIVIAL CASE : q >= 0
87 * z = 0 and w = q is solution of LCP(q,M)
88 */
89 for(int j = 0 ; j < n; j++)
90 {
91 z[j] = 0.0;
92 w[j] = q[j];
93 }
94 info = 0;
95 options->iparam[SICONOS_IPARAM_ITER_DONE] = 0; /* Number of iterations done */
96 options->dparam[SICONOS_DPARAM_RESIDU] = 0.0; /* Error */
97 if(verbose > 0)
98 printf("LCP_driver_SparseBlockMatrix: found trivial solution for the LCP (positive vector q => z = 0 and w = q). \n");
99 DEBUG_END("lcp_driver_SparseBlockMatrix(...)\n");
100 return info;
101 }
102 }
103
104 /*************************************************
105 * 2 - Call specific solver (if no trivial sol.)
106 *************************************************/
107
108 /* Solver name */
109 // const char* const name = options->solverName;
110 if(verbose == 1)
111 printf(" ========================== Call %s SparseBlockMatrix solver for Linear Complementarity problem ==========================\n", solver_options_id_to_name(options->solverId));
112
113 /****** Gauss Seidel block solver ******/
114 if((options->solverId) == SICONOS_LCP_NSGS_SBM)
115 lcp_nsgs_SBM(problem, z, w, &info, options);
116 else
117 {
118 fprintf(stderr, "LCP_driver_SparseBlockMatrix error: unknown solver named: %s\n", solver_options_id_to_name(options->solverId));
119 exit(EXIT_FAILURE);
120 }
121
122 /*************************************************
123 * 3 - Computes w = Mz + q and checks validity
124 *************************************************/
125 if(options->filterOn > 0)
126 info = lcp_compute_error(problem, z, w, options->dparam[SICONOS_DPARAM_TOL], &(options->dparam[SICONOS_DPARAM_RESIDU]));
127 DEBUG_END("lcp_driver_SparseBlockMatrix(...)\n");
128 return info;
129
130 }
131
lcp_driver_DenseMatrix(LinearComplementarityProblem * problem,double * z,double * w,SolverOptions * options)132 int lcp_driver_DenseMatrix(LinearComplementarityProblem* problem, double *z, double *w, SolverOptions* options)
133 {
134 DEBUG_BEGIN("lcp_driver_DenseMatrix(...)\n")
135 /* Note: inputs are not checked since it is supposed to be done in lcp_driver() function which calls the present one. */
136
137 /* Checks storage type for the matrix M of the LCP */
138 if(problem->M->storageType == 1)
139 numerics_error("lcp_driver_DenseMatrix", "forbidden type of storage for the matrix M of the LCP");
140
141 assert(options->isSet);
142
143 if(verbose > 0)
144 solver_options_print(options);
145
146 /* Output info. : 0: ok - >0: problem (depends on solver) */
147 int info = -1;
148 /******************************************
149 * 1 - Check for trivial solution
150 ******************************************/
151
152 int i = 0;
153 int n = problem->size;
154 double *q = problem->q;
155 if(options->iparam[SICONOS_LCP_IPARAM_SKIP_TRIVIAL] == SICONOS_LCP_SKIP_TRIVIAL_NO)
156 {
157 /* if (!((options->solverId == SICONOS_LCP_ENUM) && (options->iparam[SICONOS_IPARAM_MAX_ITER] == 1 )))*/
158 {
159 while((i < (n - 1)) && (q[i] >= 0.)) i++;
160 if((i == (n - 1)) && (q[n - 1] >= 0.))
161 {
162 /* TRIVIAL CASE : q >= 0
163 * z = 0 and w = q is solution of LCP(q,M)
164 */
165 for(int j = 0 ; j < n; j++)
166 {
167 z[j] = 0.0;
168 w[j] = q[j];
169 }
170 info = 0;
171 options->dparam[SICONOS_DPARAM_RESIDU] = 0.0; /* Error */
172 if(verbose > 0)
173 printf("LCP_driver_DenseMatrix: found trivial solution for the LCP (positive vector q => z = 0 and w = q). \n");
174 DEBUG_END("lcp_driver_DenseMatrix(...)\n")
175 return info;
176 }
177 }
178
179 // trivial solution : size-1 LCP
180 if(n == 1)
181 {
182 double *M = problem->M->matrix0;
183 w[0] = 0.;
184 z[0] = -q[0] / M[0];
185 info = 0;
186 options->dparam[SICONOS_DPARAM_RESIDU] = 0.0; /* Error */
187 if(verbose > 0)
188 printf("LCP_driver_DenseMatrix: found trivial solution for the LCP (problem of size 1). \n");
189 DEBUG_END("lcp_driver_DenseMatrix(...)\n")
190 return info;
191 }
192 }
193
194 /*************************************************
195 * 2 - Call specific solver (if no trivial sol.)
196 *************************************************/
197
198 if(verbose == 1)
199 printf(" ========================== Call %s solver for Linear Complementarity problem ==========================\n", solver_options_id_to_name(options->solverId));
200
201 /****** Lemke algorithm ******/
202 /* IN: itermax
203 OUT: iter */
204 int id = options->solverId;
205 switch(id)
206 {
207 case SICONOS_LCP_LEMKE :
208 lcp_lexicolemke(problem, z, w, &info, options);
209 break;
210 /****** PGS Solver ******/
211 /* IN: itermax, tolerance
212 OUT: iter, error */
213 case SICONOS_LCP_PGS :
214 lcp_pgs(problem, z, w, &info, options);
215 break;
216 /****** CPG Solver ******/
217 /* IN: itermax, tolerance
218 OUT: iter, error */
219 case SICONOS_LCP_CPG:
220 lcp_cpg(problem, z, w, &info, options);
221 break;
222 /****** Latin Solver ******/
223 /* IN: itermax, tolerance, k_latin
224 OUT: iter, error */
225 case SICONOS_LCP_LATIN:
226 lcp_latin(problem, z, w, &info, options);
227 break;
228 /****** Latin_w Solver ******/
229 /* IN: itermax, tolerance, k_latin, relax
230 OUT: iter, error */
231 case SICONOS_LCP_LATIN_W:
232 lcp_latin_w(problem, z, w, &info, options);
233 break;
234 /****** QP Solver ******/
235 /* IN: tolerance
236 OUT:
237 */
238 /* We assume that the LCP matrix M is symmetric*/
239 case SICONOS_LCP_QP:
240 lcp_qp(problem, z, w, &info, options);
241 break;
242 /****** NSQP Solver ******/
243 /* IN: tolerance
244 OUT:
245 */
246 case SICONOS_LCP_NSQP:
247 lcp_nsqp(problem, z, w, &info, options);
248 break;
249 /****** Newton min ******/
250 /* IN: itermax, tolerance
251 OUT: iter, error
252 */
253 case SICONOS_LCP_NEWTONMIN:
254 lcp_newton_min(problem, z, w, &info, options);
255 break;
256 /****** Newton Fischer-Burmeister ******/
257 /* IN: itermax, tolerance
258 OUT: iter, error
259 */
260 case SICONOS_LCP_NEWTON_FB_FBLSA:
261 lcp_newton_FB(problem, z, w, &info, options);
262 break;
263 /****** Newton min + Fischer-Burmeister ******/
264 /* IN: itermax, tolerance
265 OUT: iter, error
266 */
267 case SICONOS_LCP_NEWTON_MIN_FBLSA:
268 lcp_newton_minFB(problem, z, w, &info, options);
269 break;
270 /****** PSOR Solver ******/
271 /* IN: itermax, tolerance, relax
272 OUT: iter, error
273 */
274 case SICONOS_LCP_PSOR:
275 lcp_psor(problem, z, w, &info, options);
276 break;
277 /****** RPGS (Regularized Projected Gauss-Seidel) Solver ******/
278 /* IN: itermax, tolerance, rho
279 OUT: iter, error
280 */
281 case SICONOS_LCP_RPGS:
282 lcp_rpgs(problem, z, w, &info, options);
283 break;
284 /****** PATH (Ferris) Solver ******/
285 /* IN: itermax, tolerance, rho
286 OUT: iter, error
287 */
288 case SICONOS_LCP_PATH:
289 lcp_path(problem, z, w, &info, options);
290 break;
291 /****** Enumeratif Solver ******/
292 /* IN: tolerance,
293 OUT: key
294 */
295 case SICONOS_LCP_ENUM:
296 lcp_enum(problem, z, w, &info, options);
297 break;
298 /****** Reformulate as AVI and use a solver by Cao and Ferris ******/
299 /* IN: tolerance, itermax
300 OUT: iter, error
301 */
302 case SICONOS_LCP_AVI_CAOFERRIS:
303 lcp_avi_caoferris(problem, z, w, &info, options);
304 break;
305 case SICONOS_LCP_PIVOT:
306 lcp_pivot(problem, z, w, &info, options);
307 break;
308 case SICONOS_LCP_BARD:
309 options->iparam[SICONOS_LCP_IPARAM_PIVOTING_METHOD_TYPE] = SICONOS_LCP_PIVOT_BARD;
310 lcp_pivot(problem, z, w, &info, options);
311 break;
312 case SICONOS_LCP_MURTY:
313 options->iparam[SICONOS_LCP_IPARAM_PIVOTING_METHOD_TYPE] = SICONOS_LCP_PIVOT_LEAST_INDEX;
314 lcp_pivot(problem, z, w, &info, options);
315 break;
316 case SICONOS_LCP_PATHSEARCH:
317 lcp_pathsearch(problem, z, w, &info, options);
318 break;
319 case SICONOS_LCP_PIVOT_LUMOD:
320 lcp_pivot_lumod(problem, z, w, &info, options);
321 break;
322 /*error */
323 case SICONOS_LCP_GAMS:
324 lcp_gams(problem, z, w, &info, options);
325 break;
326 case SICONOS_LCP_CONVEXQP_PG:
327 lcp_ConvexQP_ProjectedGradient(problem, z, w, &info, options);
328 break;
329 default:
330 {
331 fprintf(stderr, "lcp_driver_DenseMatrix error: unknown solver name: %s\n", solver_options_id_to_name(options->solverId));
332 exit(EXIT_FAILURE);
333 }
334 }
335 /*************************************************
336 * 3 - Computes w = Mz + q and checks validity
337 *************************************************/
338 if(options->filterOn > 0)
339 {
340 int info_ = lcp_compute_error(problem, z, w, options->dparam[SICONOS_DPARAM_TOL], &(options->dparam[SICONOS_DPARAM_RESIDU]));
341 if(info <= 0) /* info was not set or the solver was happy */
342 info = info_;
343 }
344 DEBUG_END("lcp_driver_DenseMatrix(...)\n")
345 return info;
346
347 }
348
linearComplementarity_driver(LinearComplementarityProblem * problem,double * z,double * w,SolverOptions * options)349 int linearComplementarity_driver(LinearComplementarityProblem* problem, double *z, double *w, SolverOptions* options)
350 {
351 assert(options && "lcp_driver : null input for solver options");
352 DEBUG_BEGIN("linearComplementarity_driver(...)\n");
353 /* Checks inputs */
354 assert(problem && z && w &&
355 "lcp_driver : input for LinearComplementarityProblem and/or unknowns (z,w)");
356
357 /* Output info. : 0: ok - >0: problem (depends on solver) */
358 int info = -1;
359 /* Switch to DenseMatrix or SparseBlockMatrix solver according to the type of storage for M */
360 /* Storage type for the matrix M of the LCP */
361
362 NM_types storageType = problem->M->storageType;
363 DEBUG_PRINTF("storageType = %i\n", storageType);
364 /* Sparse Block Storage */
365 if(storageType == NM_SPARSE_BLOCK)
366 {
367 info = lcp_driver_SparseBlockMatrix(problem, z, w, options);
368 }
369 else
370 {
371 info = lcp_driver_DenseMatrix(problem, z, w, options);
372 }
373 DEBUG_END("linearComplementarity_driver(...)\n");
374 return info;
375 }
376