source: trunk/RuntimeLibrary/src/jmi/jmi_realtime_solver.c @ 11712

Last change on this file since 11712 was 11712, checked in by Christian Andersson, 14 months ago

Fixed segfault in CS FMUs. Related to ticket:5684

File size: 15.9 KB
Line 
1/*
2    Copyright (C) 2017 Modelon AB
3
4    This program is free software: you can redistribute it and/or modify
5    it under the terms of the GNU General Public License version 3 as published
6    by the Free Software Foundation, or optionally, under the terms of the
7    Common Public License version 1.0 as published by IBM.
8
9    This program is distributed in the hope that it will be useful,
10    but WITHOUT ANY WARRANTY; without even the implied warranty of
11    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12    GNU General Public License, or the Common Public License, for more details.
13
14    You should have received copies of the GNU General Public License
15    and the Common Public License along with this program.  If not,
16    see <http://www.gnu.org/licenses/> or
17    <http://www.ibm.com/developerworks/library/os-cpl.html/> respectively.
18*/
19
20
21#include "jmi_realtime_solver.h"
22#include "jmi_block_solver_impl.h"
23#include "jmi_linear_algebra.h"
24#include <math.h>
25#include <string.h>
26
27#define JMI_REALTIME_SOLVER_MAX_ITER 20
28#define JMI_REALTIME_UPDATE_JACOBIAN_ITER 10
29#define JMI_REALTIME_PROGRESS_LOG_LEVEL 4
30
31
32static void progress_reset_char_log(jmi_block_solver_t* block) {
33    jmi_realtime_solver_t* solver = (jmi_realtime_solver_t*)block->solver;
34   
35    solver->char_log_length = 0;
36    solver->char_log[0] = 0;
37}
38
39static void progress_char_log(jmi_block_solver_t* block, char c) {
40    jmi_realtime_solver_t* solver = (jmi_realtime_solver_t*)block->solver;
41   
42    if (block->callbacks->log_options.log_level < JMI_REALTIME_PROGRESS_LOG_LEVEL) { return; }
43   
44    if (solver->char_log_length < JMI_REALTIME_SOLVER_MAX_CHAR_LOG_LENGTH) {
45        solver->char_log[solver->char_log_length] = c;
46        solver->char_log_length++;
47        solver->char_log[solver->char_log_length] = 0;
48    } else {
49        solver->char_log[JMI_REALTIME_SOLVER_MAX_CHAR_LOG_LENGTH-1] = '?';
50    }
51}
52
53void jmi_realtime_compute_weights(jmi_real_t* x, jmi_real_t* nominals, jmi_real_t* weights, jmi_real_t tolerance, jmi_int_t N) {
54    jmi_int_t i;
55   
56    for (i = 0; i < N; i++) {
57        weights[i] = 1.0/(JMI_ABS(x[i])*tolerance + JMI_ABS(nominals[i])*tolerance);
58    }
59}
60
61void jmi_realtime_solver_delete(jmi_block_solver_t *block) {
62    jmi_realtime_solver_t* solver = (jmi_realtime_solver_t*)block->solver;
63   
64    free(solver->weights);
65    free(solver->pivots);
66    free(solver->dx);
67    free(solver->df);
68    free(solver->jacobian);
69    free(solver->factorization);
70   
71    free(solver);
72    block->solver = 0;
73}
74
75int jmi_realtime_solver_new(jmi_realtime_solver_t** solver_ptr, jmi_block_solver_t* block) {
76    jmi_realtime_solver_t* solver= (jmi_realtime_solver_t*)calloc(1,sizeof(jmi_realtime_solver_t));
77
78    if (!solver) return -1;
79   
80    solver->weights       = (jmi_real_t*)calloc(block->n,sizeof(jmi_real_t));
81    solver->pivots        = (jmi_int_t*)calloc(block->n,sizeof(jmi_int_t));
82    solver->dx            = (jmi_real_t*)calloc(block->n,sizeof(jmi_real_t));
83    solver->df            = (jmi_real_t*)calloc(block->n,sizeof(jmi_real_t));
84    solver->jacobian      = (jmi_real_t*)calloc(block->n*block->n,sizeof(jmi_real_t));
85    solver->factorization = (jmi_real_t*)calloc(block->n*block->n,sizeof(jmi_real_t));
86   
87    /* Statistics */
88    solver->nbr_non_convergence = 0;
89    solver->nbr_iterations      = 0;
90    solver->last_wrms           = 0.0;
91    solver->last_wrms_id        = -1;
92    solver->last_jacobian_rcond = -1;
93    solver->last_jacobian_norm  = -1;
94   
95    *solver_ptr = solver;
96   
97    return 0;
98}
99
100static void jmi_realtime_solver_progress(jmi_block_solver_t *block) {
101    jmi_realtime_solver_t* solver = (jmi_realtime_solver_t*)block->solver;
102    jmi_log_t *log = block->log;
103    char message[256];
104
105    if (block->callbacks->log_options.log_level < JMI_REALTIME_PROGRESS_LOG_LEVEL) { return; }
106   
107    /* Only print header first iteration */
108    if (solver->nbr_iterations == 0) { /* Do not print header if INITIAL_GUESS ok */
109        jmi_log_node(log, logInfo, "Progress", "<source:%s><message:%s><isheader:%d>",
110            "jmi_realtime_solver",
111            "iter       res_norm      max_res: ind   wrms: ind      rcond ",
112            1);
113    }
114   
115    if (solver->nbr_iterations > 0) {
116        jmi_int_t idmax = jmi_linear_algebra_idamax(block->res, block->n);
117        /* Keep the progress message on a single line by using jmi_log_enter_, jmi_log_fmt_ etc. */
118        jmi_log_node_t node = jmi_log_enter_(log, logInfo, "Progress");
119       
120        sprintf(message, "%4d%-4s%11.4e % 11.4e:%4d %11.4e:%4d %11.4e", solver->nbr_iterations, solver->char_log,
121            jmi_linear_algebra_norm(block->res,block->n), block->res[idmax], idmax, solver->last_wrms, solver->last_wrms_id, solver->last_jacobian_rcond);
122        progress_reset_char_log(block);
123
124        jmi_log_fmt_(log, node, logInfo, "<source:%s><block:%s><message:%s>",
125            "jmi_realtime_solver", block->label, message);
126        jmi_log_leave(log, node);
127    }
128    return;
129}
130
131int jmi_realtime_solver_solve(jmi_block_solver_t *block) {
132    jmi_realtime_solver_t* solver = (jmi_realtime_solver_t*)block->solver;
133    jmi_real_t tolerance = block->options->res_tol;
134    jmi_int_t ret, i;
135    jmi_log_node_t destnode={0};
136    jmi_int_t broyden_updates = block->options->jacobian_update_mode == jmi_broyden_jacobian_update_mode;
137    clock_t start_measuring = jmi_block_solver_start_clock(block);
138    clock_t jac_measuring, fac_measuring;
139    jmi_real_t elapsed_time_jac = 0.0, elapsed_time_fac = 0.0;
140   
141    /* Initialize the work vector */
142    block->F(block->problem_data,block->x,block->res,JMI_BLOCK_INITIALIZE);
143   
144    /* Initialize statistics */
145    solver->nbr_iterations = 0;
146
147    /* Evaluate */
148    ret = block->F(block->problem_data,block->x,block->res,JMI_BLOCK_EVALUATE);
149    if(ret) { jmi_realtime_solver_error_handling(block, block->x, JMI_REALTIME_SOLVER_BLOCK_EVALUATION_FAIL); return -1; }
150   
151
152    if(block->init || block->at_event || !broyden_updates) {
153        /* Compute the Jacobian (always and only done once) */
154        jac_measuring = jmi_block_solver_start_clock(block);
155        ret = jmi_realtime_solver_jacobian(block, block->res, solver->factorization);
156        elapsed_time_jac = jmi_block_solver_elapsed_time(block, jac_measuring);
157        if (ret) { jmi_realtime_solver_error_handling(block, block->x, JMI_REALTIME_SOLVER_JACOBIAN_APPROXIMATION_FAIL); return -1; }
158       
159        if (block->callbacks->log_options.log_level >= JMI_REALTIME_PROGRESS_LOG_LEVEL) {
160            solver->last_jacobian_norm = jmi_linear_algebra_dlange(solver->factorization, block->n, 'I');
161        }
162       
163        /* Broyden updates */
164        if (broyden_updates) { memcpy(solver->jacobian, solver->factorization, block->n*block->n*sizeof(jmi_real_t)); }
165       
166        /* Factorize the Jacobian */
167        fac_measuring = jmi_block_solver_start_clock(block);
168        ret = jmi_linear_algebra_LU_factorize(solver->factorization, solver->pivots, block->n);
169        elapsed_time_fac = jmi_block_solver_elapsed_time(block, fac_measuring);
170        if (ret) { jmi_realtime_solver_error_handling(block, block->x, JMI_REALTIME_SOLVER_LU_FACTORIZATION_FAIL); return -1; }
171       
172        if (block->callbacks->log_options.log_level >= JMI_REALTIME_PROGRESS_LOG_LEVEL) {
173            solver->last_jacobian_rcond = jmi_linear_algebra_dgecon(solver->factorization, solver->last_jacobian_norm, block->n, 'I');
174        }
175    }
176   
177    /* Open log and log the Jacobian.*/
178    if((block->callbacks->log_options.log_level >= 5)) {
179        destnode = jmi_log_enter_fmt(block->log, logInfo, "RealtimeSolver", 
180                                     "Realtime solver invoked for <block:%s>", block->label);
181        jmi_log_reals(block->log, destnode, logInfo, "ivs", block->x, block->n);
182        if((block->callbacks->log_options.log_level >= 6)) {
183            jmi_log_real_matrix(block->log, destnode, logInfo, "LU", solver->factorization, block->n, block->n);
184        }
185    }
186   
187    /* Iterate */
188    for (i = 0; i < JMI_REALTIME_SOLVER_MAX_ITER; i++) {
189        solver->nbr_iterations++;
190
191        /* Values x and res are current */
192        if (broyden_updates) { memcpy(solver->df, block->res, block->n*sizeof(jmi_real_t)); }
193       
194        /* Solve linear system to get the step, J_{k}*dx_{k} = F_{k} */
195        ret = jmi_linear_algebra_LU_solve(solver->factorization, solver->pivots, block->res, block->n);
196        if (ret) { jmi_realtime_solver_error_handling(block, block->x, JMI_REALTIME_SOLVER_LU_SOLVE_FAIL); return -1; }
197
198        /* Compute new x, x_{k+1} = x_{k} - dx_{k} */
199        jmi_linear_algebra_daxpy(-1.0, block->res, block->x, block->n);
200
201        /* Compute norm of the increment, dx_{k} */
202        jmi_realtime_compute_weights(block->x, block->nominal, solver->weights, tolerance, block->n);
203        solver->last_wrms = jmi_linear_algebra_wrms(solver->weights, block->res, block->n);
204       
205        /* If broyden, need to store dx_{k} */
206        if (broyden_updates) { memcpy(solver->dx, block->res, block->n*sizeof(jmi_real_t)); }
207       
208        /* Logging compute the id of the largest (weighted) step */
209        if (block->callbacks->log_options.log_level >= JMI_REALTIME_PROGRESS_LOG_LEVEL) { 
210            jmi_linear_algebra_ddot(solver->weights, block->res, block->n);
211            solver->last_wrms_id = jmi_linear_algebra_idamax(block->res, block->n);
212        }
213       
214        /* Evaluate the block with the new iteration variables */
215        ret = block->F(block->problem_data,block->x,block->res,JMI_BLOCK_EVALUATE);
216        if(ret) { jmi_realtime_solver_error_handling(block, block->x, JMI_REALTIME_SOLVER_BLOCK_EVALUATION_FAIL); return -1; }
217       
218        if (broyden_updates) {
219            /* df_{k} = f_{k+1} - f_{k} */
220            jmi_linear_algebra_daxpby(1.0, block->res, -1.0, solver->df, solver->df, block->n);
221           
222            jmi_realtime_solver_perform_broyden_update(block, solver->df, solver->dx);
223        }
224       
225        /* Progress log.*/
226        jmi_realtime_solver_progress(block);
227       
228        /* Check for convergence */
229        if (solver->last_wrms < 1.0) { break; }
230    }
231   
232    /* Close log.*/
233    if((block->callbacks->log_options.log_level >= 5)) {
234        jmi_log_reals(block->log, destnode, logInfo, "ivs", block->x, block->n);
235        jmi_log_reals(block->log, destnode, logInfo, "residual", block->res, block->n);
236        jmi_log_leave(block->log, destnode);
237    }
238   
239    if (solver->last_wrms >= 1.0) {
240        solver->nbr_non_convergence++;
241       
242        jmi_log_node(block->log, logWarning, "RealtimeNonConvergence", 
243                    "Failed to converge <block: %s> at <t: %f> due to <WRMS: %f> after <iteration: %d> and after elapsed <time: %f> whereof <Jac: %f> and <LU: %f>. Continuing...", 
244                    block->label, block->cur_time, solver->last_wrms, i, jmi_block_solver_elapsed_time(block, start_measuring), elapsed_time_jac, elapsed_time_fac);
245    } else {
246        jmi_log_node(block->log, logInfo, "RealtimeConvergence", 
247                    "Succeeded to converge <block: %s> at <t: %f> with <WRMS: %f> after <iteration: %d> and after elapsed <time: %f> whereof <Jac: %f> and <LU: %f>.", 
248                    block->label, block->cur_time, solver->last_wrms, i, jmi_block_solver_elapsed_time(block, start_measuring), elapsed_time_jac, elapsed_time_fac);
249    }
250   
251    return 0;
252}
253
254int jmi_realtime_solver_perform_broyden_update(jmi_block_solver_t *block, jmi_real_t* df, jmi_real_t* dx) {
255    jmi_realtime_solver_t* solver = (jmi_realtime_solver_t*)block->solver;
256    jmi_int_t ret;
257
258    jmi_realtime_solver_broyden_update(solver->jacobian, df, dx, block->n);
259    memcpy(solver->factorization, solver->jacobian, block->n*block->n*sizeof(jmi_real_t));
260   
261    progress_char_log(block, 'B');
262   
263    /* Factorize the Jacobian */
264    ret = jmi_linear_algebra_LU_factorize(solver->factorization, solver->pivots, block->n);
265    if (ret) { jmi_realtime_solver_error_handling(block, block->x, JMI_REALTIME_SOLVER_LU_FACTORIZATION_FAIL); return -1; }
266   
267    return 0;
268}
269
270/*
271 * J_n1 = Jn + (df - Jn dx) / ||dx||^2 dx
272 */
273int jmi_realtime_solver_broyden_update(jmi_real_t* jacobian, jmi_real_t* df, jmi_real_t* dx, jmi_int_t N) {
274    jmi_real_t norm, norm_inv;
275   
276    /* Computes -Jn dx + df */
277    jmi_linear_algebra_dgemv(1.0, jacobian, dx, 1.0, df, N, FALSE);
278   
279    /* 1 / || dx || ^ 2 */
280    norm = jmi_linear_algebra_ddot(dx, dx, N);
281   
282    if (norm < 1e-14) { norm_inv = 0.0; } else { norm_inv = -1.0/norm; }
283   
284    /* J_n1 = Jn + 1/||dx||^2 * tmp * dx^T */
285    jmi_linear_algebra_dger(norm_inv, df, dx, jacobian, N);
286   
287    return 0;
288}
289
290int jmi_realtime_solver_jacobian(jmi_block_solver_t *block, jmi_real_t* f, jmi_real_t* jacobian) {
291    jmi_real_t eps = block->options->jacobian_finite_difference_delta;
292    jmi_int_t ret, i;
293   
294   
295    /*
296    jmi_real_t fnorm, min_inc;
297    jmi_realtime_solver_t* solver = (jmi_realtime_solver_t*)block->solver;
298    jmi_realtime_compute_weights(block->x, block->nominal, solver->weights, block->options->res_tol, block->n);
299   
300    fnorm   = jmi_linear_algebra_wrms(solver->weights, f, block->n);
301    min_inc = (fnorm != 0.0) ? (1000 * 0.001 * 1e-16 * block->n * fnorm) : 1.0;
302    */
303   
304    progress_char_log(block, 'J');
305   
306    for (i = 0; i < block->n; i++) {
307        jmi_real_t xi   = block->x[i];
308        jmi_real_t sign = JMI_SIGN(xi); 
309        jmi_real_t inc  = sign*eps*JMI_MAX(JMI_ABS(xi), JMI_ABS(block->nominal[i]));
310        /*jmi_real_t inc  = sign*JMI_MAX(eps*JMI_ABS(xi), min_inc/solver->weights[i]);  */
311        jmi_real_t inc_inv;
312       
313        block->x[i] += inc;
314       
315        ret = block->F(block->problem_data,block->x,block->dres,JMI_BLOCK_EVALUATE);
316        if (ret) {
317            inc = -inc;
318            block->x[i] = xi + inc;
319            ret = block->F(block->problem_data,block->x,block->dres,JMI_BLOCK_EVALUATE);
320        }
321        if (ret) {
322            /* Error handling... */
323            return -1;
324        }
325       
326        inc_inv = 1.0 / inc;
327        jmi_linear_algebra_daxpby(inc_inv, block->dres, -inc_inv, f, &(jacobian[i*(block->n)]), block->n); 
328       
329        block->x[i] = xi;
330    }
331
332    return 0;
333}
334
335
336void jmi_realtime_solver_error_handling(jmi_block_solver_t *block, jmi_real_t* x, jmi_realtime_solver_error_codes_t return_code) {
337    jmi_log_node_t node={0};
338   
339   
340    if (return_code == JMI_REALTIME_SOLVER_BLOCK_EVALUATION_FAIL) {
341        node = jmi_log_enter_fmt(block->log, logWarning, "BlockEvaluationFailed", 
342                "Failed to evaulation block function, <errorCode: %d> returned in <block: %s> at <t: %f>.", 
343                return_code, block->label, block->cur_time);
344    } else if (return_code == JMI_REALTIME_SOLVER_JACOBIAN_APPROXIMATION_FAIL) {
345        node = jmi_log_enter_fmt(block->log, logWarning, "BlockJacobianFailed", 
346                "Failed to approximate the block Jacobian, <errorCode: %d> returned in <block: %s> at <t: %f>.",
347                return_code, block->label, block->cur_time);
348    } else if (return_code == JMI_REALTIME_SOLVER_LU_FACTORIZATION_FAIL) {
349        node = jmi_log_enter_fmt(block->log, logWarning, "BlockJacobianFactorizationFailed", 
350                "Failed to perform the LU factorization of the block Jacobian, <errorCode: %d> returned in <block: %s> at <t: %f>.",
351                return_code, block->label, block->cur_time);
352    } else if (return_code == JMI_REALTIME_SOLVER_LU_SOLVE_FAIL) {
353        node = jmi_log_enter_fmt(block->log, logWarning, "BlockJacobianSolveFailed", 
354                "Failed to solve the linear system using the LU factorization of the block Jacobian, <errorCode: %d> returned in <block: %s> at <t: %f>.",
355                return_code, block->label, block->cur_time);
356    } else {
357        node = jmi_log_enter_fmt(block->log, logWarning, "UnknownFailure", 
358                "Unknown <errorCode: %d> returned from the realtime solver in <block: %s> at <t: %f>.", 
359                return_code, block->label, block->cur_time);
360    }
361   
362   
363    if (block->callbacks->log_options.log_level >= 3) { 
364        jmi_log_reals(block->log, node, logWarning, "ivs", x, block->n); 
365    }
366
367    jmi_log_leave(block->log, node);
368}
Note: See TracBrowser for help on using the repository browser.