source: branches/dev-cw-2658/RuntimeLibrary/src/jmi/jmi_realtime_solver.c @ 14000

Last change on this file since 14000 was 14000, checked in by Christian Andersson, 4 weeks ago

Minor cleanup. Related to ticket:5868

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