1
2 /* Siconos is a program dedicated to modeling, simulation and control
3 * of non smooth dynamical systems.
4 *
5 * Copyright 2021 INRIA.
6 *
7 * Licensed under the Apache License, Version 2.0 (the "License");
8 * you may not use this file except in compliance with the License.
9 * You may obtain a copy of the License at
10 *
11 * http://www.apache.org/licenses/LICENSE-2.0
12 *
13 * Unless required by applicable law or agreed to in writing, software
14 * distributed under the License is distributed on an "AS IS" BASIS,
15 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
16 * See the License for the specific language governing permissions and
17 * limitations under the License.
18 */
19 #include <assert.h> // for assert
20 #include <math.h> // for sqrt
21 #include <stdio.h> // for printf, fpr...
22 #include <stdlib.h> // for free, malloc
23 #include "FrictionContactProblem.h" // for FrictionCon...
24 #include "Friction_cst.h" // for SICONOS_FRI...
25 #include "NumericsFwd.h" // for SolverOptions
26 #include "SOCLCP_Solvers.h" // for soclcp_VI_E...
27 #include "SOCLCP_cst.h" // for SICONOS_SOC...
28 #include "SecondOrderConeLinearComplementarityProblem.h" // for SecondOrder...
29 #include "SiconosBlas.h" // for cblas_dnrm2
30 #include "SolverOptions.h" // for SolverOptions
31 #include "VI_cst.h" // for SICONOS_VI_...
32 #define DEBUG_MESSAGES
33 #define DEBUG_STDOUT
34 #include "siconos_debug.h" // lines 32-32
35 #include "fc3d_Solvers.h" // for fc3d_set_in...
36 #include "fc3d_compute_error.h" // for fc3d_comput...
37 #include "numerics_verbose.h" // for verbose
38
39
40 /** pointer to function used to call internal solver for proximal point solver */
41 typedef void (*soclcp_InternalSolverPtr)(SecondOrderConeLinearComplementarityProblem*, double*, double*, int *, SolverOptions *);
42
fc3d_ACLMFixedPoint(FrictionContactProblem * problem,double * reaction,double * velocity,int * info,SolverOptions * options)43 void fc3d_ACLMFixedPoint(FrictionContactProblem* problem, double *reaction, double *velocity, int* info, SolverOptions* options)
44 {
45 /* int and double parameters */
46 int* iparam = options->iparam;
47 double* dparam = options->dparam;
48
49 /* Number of contacts */
50 int nc = problem->numberOfContacts;
51
52
53 /* Maximum number of iterations */
54 int itermax = iparam[SICONOS_IPARAM_MAX_ITER];
55 /* Tolerance */
56 double tolerance = dparam[SICONOS_DPARAM_TOL];
57 double norm_q = cblas_dnrm2(nc*3, problem->q, 1);
58
59
60
61 if(options->numberOfInternalSolvers < 1)
62 {
63 numerics_error("fc3d_ACLMFixedpoint", "The ACLM Fixed Point method needs options for the internal solvers, please check your options.");
64 }
65
66 SolverOptions * internalsolver_options = options->internalSolvers[0];
67
68 if(verbose > 0)
69 {
70 solver_options_print(options);
71 }
72
73
74 /***** Fixed Point Iterations *****/
75 int iter = 0; /* Current iteration number */
76 double error = 1.; /* Current error */
77 int hasNotConverged = 1;
78 soclcp_InternalSolverPtr internalsolver;
79
80 SecondOrderConeLinearComplementarityProblem* soclcp = (SecondOrderConeLinearComplementarityProblem *)malloc(sizeof(SecondOrderConeLinearComplementarityProblem));
81 soclcp->n = problem->numberOfContacts * problem->dimension;
82 soclcp->nc= problem->numberOfContacts;
83 soclcp->M = problem-> M;
84 soclcp->q = (double *) malloc(soclcp->n * sizeof(double));
85 soclcp->tau = problem->mu;
86 soclcp->coneIndex = (unsigned int *) malloc((soclcp->nc+1) * sizeof(unsigned int));
87
88 for(int i=0; i < soclcp->n; i++)
89 {
90 soclcp->q[i]=problem->q[i];
91 }
92 for(int i=0; i < soclcp->nc+1; i++)
93 {
94 soclcp->coneIndex[i] = 3*i;
95 }
96
97 if(internalsolver_options->solverId == SICONOS_SOCLCP_NSGS)
98 {
99 if(verbose == 1)
100 printf(" ========================== Call NSGS solver SOCLCP problem ==========================\n");
101 internalsolver = &soclcp_nsgs;
102 }
103 else if(internalsolver_options->solverId == SICONOS_SOCLCP_VI_FPP)
104 {
105 if(verbose == 1)
106 printf(" ========================== Call VI_FPP solver SOCLCP problem ==========================\n");
107 internalsolver = &soclcp_VI_FixedPointProjection;
108 }
109 else if(internalsolver_options->solverId == SICONOS_SOCLCP_VI_EG)
110 {
111 if(verbose == 1)
112 printf(" ========================== Call VI_EG solver SOCLCP problem ==========================\n");
113 internalsolver = &soclcp_VI_ExtraGradient;
114 }
115 else
116 {
117 fprintf(stderr, "Numerics, fc3d_ACLMFixedPoint failed. Unknown internal solver.\n");
118 exit(EXIT_FAILURE);
119 }
120
121 double normUT;
122 int cumul_iter =0;
123 while((iter < itermax) && (hasNotConverged > 0))
124 {
125 ++iter;
126 // internal solver for the regularized problem
127
128 /* Compute the value of the initial value of q */
129 for(int ic = 0 ; ic < nc ; ic++)
130 {
131 normUT = sqrt(velocity[ic*3+1] * velocity[ic*3+1] + velocity[ic*3+2] * velocity[ic*3+2]);
132 soclcp->q[3*ic] = problem->q[3*ic] + problem->mu[ic]*normUT;
133 }
134
135 fc3d_set_internalsolver_tolerance(problem,options,internalsolver_options, error);
136
137
138 (*internalsolver)(soclcp, reaction, velocity, info, internalsolver_options);
139 cumul_iter += internalsolver_options->iparam[SICONOS_IPARAM_ITER_DONE];
140 /* **** Criterium convergence **** */
141
142 fc3d_compute_error(problem, reaction, velocity, tolerance, options, norm_q, &error);
143
144 if(options->callback)
145 {
146 options->callback->collectStatsIteration(options->callback->env, nc * 3,
147 reaction, velocity, error, NULL);
148 }
149
150 if(verbose > 0)
151 printf("---- FC3D - ACLMFP - Iteration %i Residual = %14.7e\n", iter, error);
152
153 if(error < tolerance) hasNotConverged = 0;
154 *info = hasNotConverged;
155 }
156 if(verbose > 0)
157 {
158 printf("--------------- FC3D - ACLMFP - # Iteration %i Final Residual = %14.7e\n", iter, error);
159 printf("--------------- FC3D - ACLMFP - # internal iteration = %i\n", cumul_iter);
160 }
161 free(soclcp->q);
162 free(soclcp->coneIndex);
163 free(soclcp);
164
165 dparam[SICONOS_VI_DPARAM_RHO] = internalsolver_options->dparam[SICONOS_VI_DPARAM_RHO];
166 dparam[SICONOS_DPARAM_RESIDU] = error;
167 iparam[SICONOS_IPARAM_ITER_DONE] = iter;
168
169 }
170
171
172
fc3d_aclmfp_set_default(SolverOptions * options)173 void fc3d_aclmfp_set_default(SolverOptions* options)
174 {
175 options->iparam[SICONOS_FRICTION_3D_IPARAM_INTERNAL_ERROR_STRATEGY] = SICONOS_FRICTION_3D_INTERNAL_ERROR_STRATEGY_ADAPTIVE;
176 options->dparam[SICONOS_FRICTION_3D_DPARAM_INTERNAL_ERROR_RATIO] =10.0;
177
178 assert(options->numberOfInternalSolvers == 1);
179 options->internalSolvers[0] = solver_options_create(SICONOS_SOCLCP_NSGS);
180 options->internalSolvers[0]->iparam[SICONOS_IPARAM_MAX_ITER] =10000;
181 }
182