df2823027804179c091c9034fd3dad80a8c387e5
[scilab.git] / scilab / modules / scicos / macros / scicos_auto / lincos.sci
1 //  Scicos
2 //
3 //  Copyright (C) INRIA - METALAU Project <scicos@inria.fr>
4 //  Copyright (C) DIGITEO - ClĂ©ment DAVID <clement.david@scilab.org>
5 //
6 // This program is free software; you can redistribute it and/or modify
7 // it under the terms of the GNU General Public License as published by
8 // the Free Software Foundation; either version 2 of the License, or
9 // (at your option) any later version.
10 //
11 // This program is distributed in the hope that it will be useful,
12 // but WITHOUT ANY WARRANTY; without even the implied warranty of
13 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14 // GNU General Public License for more details.
15 //
16 // You should have received a copy of the GNU General Public License
17 // along with this program; if not, write to the Free Software
18 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
19 //
20 // See the file ./license.txt
21 //
22
23 function sys = lincos(scs_m,x0,u0,param)
24     // NAME
25     // lincos - Constructs by linearization a linear state-space
26     // model from a general dynamical system described by a
27     // scicos diagram
28
29     // SYNTAX
30     //
31     // sys = lincos(scs_m [,x0,u0 [,param] ])
32     //
33     //
34     // PARAMETERS
35     //
36     // scs_m: a Scicos data structure
37     // x0: column vector. Continuous state around which linearization to be done (default 0)
38     // u0: column vector. Input around which linearization to be done (default 0)
39     // param: list with two elements (default list(1.d-6,0))
40     //   param(1): scalar. Perturbation level for linearization; the following variation is used
41     //             del([x;u])_i = param(1)+param(1)*1d-4*abs([x;u])_i
42     //   param(2): scalar. Time t.
43     //
44     // sys: state-space system
45     //
46     // DESCRIPTION
47     // Constructs by linearization a linear state-space
48     // model from a general dynamical system described by a
49     // scicos diagram scs_m. Input and output ports, normally
50     // used inside superblocks, should be used to specify
51     // inputs and outputs in the scicos diagram. Suppose the
52     // scicos diagram to be linearized is called mysystem and
53     // it is saved in mysystem.cos in the current directory. The scicos
54     // diagram scs_m can be obtained either by
55     //             scs_m = scicos('mysystem.cos');
56     // followed by a quit in the scicos menu, or by
57     //             load('mysystem.cos')
58     // which creates by default a variable called scs_m.
59
60
61     //** This function can be (ab)used from the Scilab command line and
62     //** inside a Scicos "context". In order to handle the different situations,
63     //** the required libraries are loaded if not already present in the
64     //** "semiglobal-local-environment".
65
66     if ~exists("scicos_diagram") then
67         loadXcosLibs();
68     end
69
70     if exists("scicos_scicoslib")==0 then
71         load("SCI/modules/scicos/macros/scicos_scicos/lib") ;
72     end
73
74     if exists("scicos_autolib")==0 then
75         load("SCI/modules/scicos/macros/scicos_auto/lib") ;
76     end
77
78     if exists("scicos_utilslib")==0 then
79         load("SCI/modules/scicos/macros/scicos_utils/lib") ;
80     end
81
82     // Define Scicos data tables ===========================================
83     if ( ~isdef("modelica_libs") | ..
84         ~isdef("scicos_pal_libs") ) then
85         [modelica_libs, scicos_pal_libs, %scicos_with_grid, %scs_wgrid] = initial_scicos_tables();
86     end
87     // =====================================================================
88     [lhs, rhs] = argn(0);
89
90     if rhs < 1 then
91         error(msprintf(gettext("%s: Wrong number of input argument(s): At least %d expected.\n"), "lincos", 1));
92     end
93     if typeof(scs_m)<>"diagram" & typeof(scs_m)<>"cpr" then
94         error(msprintf(gettext("%s: Wrong type for input argument #%d: A diagram expected.\n"), "lincos", 1));
95     end
96
97     // compile and post-process the diagram
98     if typeof(scs_m)=="diagram" then
99
100         // Propagate context through all blocks
101         %state0     = list();
102         needcompile = 4;
103         %cpr        = struct();
104         %cpr.state  = %state0;
105         %scicos_context = struct();
106         context = scs_m.props.context;
107
108         [%scicos_context, ierr] = script2var(context, %scicos_context);
109         [scs_m,%cpr,needcompile,ok] = do_eval(scs_m, %cpr, %scicos_context);
110         if ~ok then
111             error(msprintf(gettext("%s: Error during block parameters evaluation.\n"), "lincos"));
112         end
113
114         IN  = [];
115         OUT = [];
116
117         // check version
118         current_version = get_scicos_version()
119         scicos_ver      = find_scicos_version(scs_m)
120         if scicos_ver<>current_version then
121             scs_m=do_version(scs_m, scicos_ver);
122         end
123
124         // looking for I/O
125         for i=1:size(scs_m.objs)
126             if typeof(scs_m.objs(i))=="Block" then
127                 if or(scs_m.objs(i).gui==["IN_f", "INPUTPORT"]) then
128                     scs_m.objs(i).gui="INPUTPORT";
129                     IN=[IN scs_m.objs(i).model.ipar]
130                 elseif or(scs_m.objs(i).gui==["OUT_f", "OUTPUTPORT"]) then
131                     scs_m.objs(i).gui="OUTPUTPORT";
132                     OUT=[OUT  scs_m.objs(i).model.ipar]
133                 elseif or(scs_m.objs(i).gui==["CLKIN_f","CLKINV_f", "INPUTPORTEVTS"]) then
134                     scs_m.objs(i).gui="INPUTPORTEVTS";
135                     scs_m.objs(i).model.sim(1)="bidon"
136                 elseif  or(scs_m.objs(i).gui==["CLKOUT_f","CLKOUTV_f", "OUTPUTPORTEVTS"]) then
137                     scs_m.objs(i).gui="OUTPUTPORTEVTS";
138                     scs_m.objs(i).model.sim(1)="bidon"
139                 end
140             end
141         end
142
143         if IN == [] then
144             error(msprintf(gettext("%s: Unable to find diagram inputs\n"), "lincos"));
145         end
146         if OUT == [] then
147             error(msprintf(gettext("%s: Unable to find diagram outputs\n"), "lincos"));
148         end
149
150         IN=-gsort(-IN);
151         if or(IN<>[1:size(IN,"*")]) then
152             error(msprintf(gettext("%s: Input ports are not numbered properly.\n"),"lincos"))
153         end
154
155         OUT=-gsort(-OUT);
156         if or(OUT<>[1:size(OUT,"*")]) then
157             error(msprintf(gettext("%s: Output ports are not numbered properly.\n"),"lincos"))
158         end
159
160         //compile scs_m
161         [bllst, connectmat, clkconnect,cor,corinv,ok] = c_pass1(scs_m);
162         if ~ok then
163             error(msprintf(gettext("%s: Diagram does not compile in pass %d.\n"),"lincos",1));
164         end
165
166         %cpr = c_pass2(bllst,connectmat,clkconnect,cor,corinv);
167
168         if %cpr==list() then
169             ok = %f ;
170         end
171
172         if ~ok then
173             error(msprintf(gettext("%s: Diagram does not compile in pass %d.\n"),"lincos",2))
174         end
175
176         // compile and post-process the diagram end
177     end
178
179     sim   = %cpr.sim;
180     state = %cpr.state;
181     //
182     inplnk = sim.inplnk;
183     inpptr = sim.inpptr;
184     outlnk = sim.outlnk;
185     outptr = sim.outptr;
186     ipptr  = sim.ipptr;
187
188     ki=[];ko=[];nyptr=1;
189     for kfun=1:length(sim.funs)
190         if sim.funs(kfun)=="output" then
191             sim.funs(kfun)="bidon"
192             ko=[ko;[kfun,sim.ipar(ipptr(kfun))]];
193         elseif sim.funs(kfun)=="input" then
194             sim.funs(kfun)="bidon"
195             ki=[ki;[kfun,sim.ipar(ipptr(kfun))]];
196         end
197     end
198
199     [junk,ind]=gsort(-ko(:,2));ko=ko(ind,1);
200     [junk,ind]=gsort(-ki(:,2));ki=ki(ind,1);
201
202     pointo=[];
203     for k=ko'
204         pointo=[pointo;inplnk(inpptr(k))]
205     end
206     pointi=[];
207
208     for k=ki'
209         pointi=[pointi;outlnk(outptr(k))]
210     end
211
212     nx=size(state.x,"*");
213     nu=0; for k=pointi', nu=nu+size(state.outtb(k),"*"), end
214     ny=0; for k=pointo', ny=ny+size(state.outtb(k),"*"), end
215
216     if rhs<3 then
217         x0=zeros(nx,1);u0=zeros(nu,1);
218     else
219         if size(x0,"*")<>nx then
220             if nx == 0 then
221                 error(msprintf(gettext("%s: Wrong size for input argument #%d: Empty matrix expected.\n"), "lincos", 2));
222             else
223                 error(msprintf(gettext("%s: Wrong size for input argument #%d: %d-by-%d matrix expected.\n"),"lincos", 2, nx, 1))
224             end
225         elseif size(u0,"*")<>nu then
226             if nx == 0 then
227                 error(msprintf(gettext("%s: Wrong size for input argument #%d: Empty matrix expected.\n"), "lincos", 3));
228             else
229                 error(msprintf(gettext("%s: Wrong size for input argument #%d: %d-by-%d matrix expected.\n"),"lincos", 3, nx, 1))
230             end
231         end
232     end
233
234     if rhs==4 then
235         del = param(1)+param(1)*1d-4*abs([x0;u0])
236         t   = param(2)
237     else
238         del = 1.d-6*(1+1d-4*abs([x0;u0]) )
239         t   = 0;
240     end
241
242     x0 = x0(:);
243     u0 = u0(:);
244
245     state.x=x0;
246
247     Uind = 1
248     for k=pointi'
249         state.outtb(k) = matrix(u0(Uind:Uind+size(state.outtb(k),"*")-1),size(state.outtb(k)));
250         Uind = size(state.outtb(k),"*")+1;
251     end
252
253     [state,t]=scicosim(state,t,t,sim,"start",[.1,.1,.1,.1]);
254     [state,t]=scicosim(state,t,t,sim,"linear",[.1,.1,.1,.1]);
255     Yind=1
256     for k=pointo'
257         y0(Yind:Yind+size(state.outtb(k),"*")-1)=state.outtb(k)(:);
258         Yind=size(state.outtb(k),"*")+1
259     end
260     xp0=state.x;
261     zo0=[xp0;y0];
262
263     F=zeros(nx+ny,nx+nu);
264     z0=[x0;u0];
265     zer=zeros(nx+nu,1);
266
267     for i=1:nx+nu
268         dz=zer;dz(i)=del(i);
269         z=z0+dz;
270         state.x=z(1:nx);
271         Uind=nx+1
272         for k=pointi'
273             state.outtb(k)=matrix(z(Uind:Uind+size(state.outtb(k),"*")-1),size(state.outtb(k)));
274             Uind=size(state.outtb(k),"*")+1;
275         end
276         [state,t]=scicosim(state,t,t,sim,"linear",[.1,.1,.1,.1]);
277         zo=[]
278         Yind=1
279         for k=pointo'
280             zo(Yind:Yind+size(state.outtb(k),"*")-1)=state.outtb(k)(:);
281             Yind=size(state.outtb(k),"*")+1
282         end
283         zo=[state.x;zo];
284         F(:,i)=(zo-zo0)/del(i);
285     end
286     [state,t]=scicosim(state,t,t,sim,"finish",[.1,.1,.1,.1]);
287
288     sys = syslin("c",F(1:nx,1:nx),F(1:nx,nx+1:nx+nu),F(nx+1:nx+ny,1:nx),F(nx+1:nx+ny,nx+1:nx+nu));
289
290 endfunction