Simplify parameter fungsi formationControlBuilder dengan struck yang disimpan ke file agar supaya dapat diakses disemua fungsi
algo_init implemented and add geogebra file for ilustration.master
parent
b17e689911
commit
acf4e14e37
|
|
@ -0,0 +1 @@
|
|||
/algo_info.m
|
||||
|
|
@ -1,29 +1,38 @@
|
|||
clear -all
|
||||
|
||||
graphics_toolkit('gnuplot')
|
||||
omni = omni = omnidirectional(0.94, 0.96, 0.01, 2.2, 1.5,0.099,0.1,1.5,0.025,30,0.035,19,0.00011,1.69,0.0059,0.0059,0.9,9.1,6,'./');
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% config nang kene
|
||||
conRobot = [1 2 1;
|
||||
3 2 1;
|
||||
3 1 1;
|
||||
];
|
||||
length_d = 3;
|
||||
dRobot = [ 1;
|
||||
1;
|
||||
1;
|
||||
];
|
||||
corRobot = [1.5; 1.7;0;
|
||||
2; 2.5;0;
|
||||
2.5; 2.8;0;
|
||||
algo = 0;
|
||||
d = [[]];
|
||||
ang = [];
|
||||
save_state = [];
|
||||
save algo_info.m algo;
|
||||
save algo_info.m "-append" d ang save_state;
|
||||
bParam = struct();
|
||||
bParam.model = omnidirectional(0.94, 0.96, 0.01, 2.2, 1.5,0.099,0.1,1.5,0.025,30,0.035,19,0.00011,1.69,0.0059,0.0059,0.9,9.1,6,'./');
|
||||
bParam.edgeL = [1 2 1;
|
||||
3 2 1;
|
||||
3 1 1;
|
||||
];
|
||||
bParam.dScale = 3;
|
||||
bParam.kp1 = 100;
|
||||
bParam.kp2 = 100;
|
||||
bParam.ki1 = 0;
|
||||
bParam.ki2 = 0;
|
||||
bParam.nNodeVref = 1;
|
||||
|
||||
|
||||
corRobot = [0; 0;0;
|
||||
-1; -2;0;
|
||||
-3; -3;0;
|
||||
]; % xy xy xy
|
||||
robStateLen = size(omni.origin.A,1);
|
||||
robStateLen = size(bParam.model.origin.A,1);
|
||||
B = [
|
||||
1 0; 0 1;
|
||||
0 0; 0 0;
|
||||
0 0; 0 0;
|
||||
];
|
||||
kp = 1;
|
||||
ki = 0;
|
||||
|
||||
% fvref = fncSpeedRef('ysin',50,2,robStateLen);
|
||||
% fvref = fncSpeedRef('xsin',50,2,robStateLen);
|
||||
% fvref = fncSpeedRef('cw',-300,1,robStateLen);
|
||||
|
|
@ -31,30 +40,31 @@ ki = 0;
|
|||
fvref = fncSpeedRef('s',0,0,robStateLen);
|
||||
h = .01;
|
||||
tspan = 1:h:10;
|
||||
% sys = formationControl2ndBuilder(conRobot,robStateLen,kp,kp,ki,ki,length_d,1);
|
||||
% sys = formationControl1stBuilder(conRobot,robStateLen,kp,kp,ki,ki,length_d,1);
|
||||
% sys = formationControl2ndModelBuilder(conRobot,omni.origin,kp,kp,ki,ki,length_d,1);
|
||||
sys = formationControl1stModelBuilder(conRobot,omni.origin,kp,kp,ki,ki,length_d,1);
|
||||
|
||||
% sys = formationControl2ndBuilder(bParam);
|
||||
% sys = formationControl1stBuilder(bParam);
|
||||
sys = formationControl2ndModelBuilder(bParam);
|
||||
% sys = formationControl1stModelBuilder(bParam);
|
||||
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% config nang nduwur
|
||||
|
||||
_zero = zeros(size(sys.R(corRobot,sys.K),1),1);
|
||||
sInit = [corRobot; _zero;];
|
||||
s1stInit = [corRobot; zeros(size(corRobot)); zeros(size(dRobot))];
|
||||
s1stInit = [corRobot; zeros(size(corRobot)); zeros(size(sys.d))];
|
||||
s2ndInit = [corRobot;zeros(length(corRobot),1); zeros(length(corRobot),1); _zero;];
|
||||
s2ndModelInit = [corRobot; zeros(length(corRobot),1); zeros(length(corRobot),1); _zero; zeros(length(corRobot),1);];
|
||||
|
||||
% state_init = s2ndInit;
|
||||
% state_init = sInit;
|
||||
% state_init = s2ndModelInit;
|
||||
state_init = s1stInit;
|
||||
state_init = s2ndModelInit;
|
||||
% state_init = s1stInit;
|
||||
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% start solving
|
||||
printf("Mulai memecakan masalah \n\n");
|
||||
startExe = tic;
|
||||
|
||||
% continue
|
||||
dydt = @(t, y) sys.ss(y,fvref(t));
|
||||
dydt = @(t, y) algo_init(sys,y,fvref(t));
|
||||
[t,y] = ode45(dydt, tspan, state_init);
|
||||
|
||||
% discrate taylor methode
|
||||
|
|
@ -121,13 +131,13 @@ function plot_con (pltRb, yOut, conIn,xm,ym, time)
|
|||
endfor
|
||||
endfunction
|
||||
|
||||
plot_con(plot_rb, y', conRobot, xrb, yrb, 1);
|
||||
% plot_con(plot_rb, y', conRobot, xrb, yrb, round(length(tspan)/2));
|
||||
plot_con(plot_rb, y', conRobot, xrb, yrb, length(tspan));
|
||||
plot_con(plot_rb, y', bParam.edgeL, xrb, yrb, 1);
|
||||
% plot_con(plot_rb, y', bParam.edgeL, xrb, yrb, round(length(tspan)/2));
|
||||
plot_con(plot_rb, y', bParam.edgeL, xrb, yrb, length(tspan));
|
||||
|
||||
|
||||
str_tmp = "legend(";
|
||||
for i = 1:numNodes(edgeL2adjL(conRobot))-1;
|
||||
for i = 1:numNodes(edgeL2adjL(bParam.edgeL))-1;
|
||||
str_tmp =strcat(str_tmp,sprintf("\"R%i \", ",i));
|
||||
endfor
|
||||
str_tmp =strcat(str_tmp,sprintf("\"R%i \" )",++i));
|
||||
|
|
|
|||
|
|
@ -0,0 +1,100 @@
|
|||
function state = algo_init(obj, st, vRef)
|
||||
algo_info = load("algo_info.m");
|
||||
algo = algo_info.algo;
|
||||
d = algo_info.d;
|
||||
ang = algo_info.ang;
|
||||
save_state = algo_info.save_state;
|
||||
fDeggreCosine = @(a,b,c) acosd(((a^2)+(b^2)-(c^2)) ...
|
||||
/(2*a*b));
|
||||
switch(algo);
|
||||
case 0;
|
||||
|
||||
d(1,1) = norm((obj.K*st(1:obj.nodeStateLength))(1:3));
|
||||
d(2,1) = norm((obj.K*st(1:obj.nodeStateLength))(4:6));
|
||||
algo = 1;
|
||||
state = st;
|
||||
al_state = [st(1:3)' st(obj.nodeStateLength+1:obj.nodeStateLength+3)']';
|
||||
|
||||
case 1;
|
||||
set_point = [2 0 0 0 0 0]';
|
||||
state = obj.Av2(st,[1 0 0])*st + obj.B(st)*obj.d + obj.Kb*vRef;
|
||||
al_state = [st(1:3)' st(obj.nodeStateLength+1:obj.nodeStateLength+3)']';
|
||||
if norm(al_state - set_point) < .1
|
||||
d(1,2) = norm((obj.K*st(1:obj.nodeStateLength))(1:3)); ... from sensor
|
||||
d(2,2) = norm((obj.K*st(1:obj.nodeStateLength))(4:6));
|
||||
ang(1) = 180 - fDeggreCosine(al_state(1),d(1,2),d(1,1));
|
||||
ang(2) = 180 - fDeggreCosine(al_state(1),d(2,2),d(2,1));
|
||||
save_state = al_state;
|
||||
printf("<algo_init>Estimate stage 1 \n");
|
||||
cal_al_state = [d(1,2)*cosd(ang(1))+al_state(1);
|
||||
d(1,2)*sind(ang(1))+al_state(2);
|
||||
0 ;
|
||||
d(2,2)*cosd(ang(2))+al_state(1);
|
||||
d(2,2)*sind(ang(2))+al_state(2);
|
||||
0]
|
||||
algo=2;
|
||||
else
|
||||
al_state = obj.param.model.A * al_state ...
|
||||
+ obj.param.model.B ...
|
||||
* arrayfun(obj.param.model.limit_motor, ...
|
||||
(obj.param.model.Nst * set_point ...
|
||||
- obj.param.model.Kst*al_state ));
|
||||
state(1:3) = al_state(1:3);
|
||||
state(obj.nodeStateLength+1:obj.nodeStateLength+3) = al_state(4:6);
|
||||
endif
|
||||
case 2;
|
||||
set_point = [1 1 0 0 0 0]';
|
||||
state = obj.Av2(st,[1 0 0])*st + obj.B(st)*obj.d + obj.Kb*vRef;
|
||||
al_state = [st(1:3)' st(obj.nodeStateLength+1:obj.nodeStateLength+3)']';
|
||||
if norm(al_state - set_point) < .1
|
||||
printf("<algo_init>Estimate stage 2 \n");
|
||||
cal_al_state = [al_state(1:3);
|
||||
d(1,2)*cosd(ang(1))+save_state(1);
|
||||
d(1,2)*sind(ang(1))+save_state(2);
|
||||
0 ;
|
||||
d(2,2)*cosd(ang(2))+save_state(1);
|
||||
d(2,2)*sind(ang(2))+save_state(2);
|
||||
0];
|
||||
d(1,3) = norm((obj.K*st(1:obj.nodeStateLength))(1:3)); ... from sensor
|
||||
d(2,3) = norm((obj.K*st(1:obj.nodeStateLength))(4:6));
|
||||
_d(1) = norm((obj.K*cal_al_state(1:obj.nodeStateLength))(1:3));
|
||||
_d(2) = norm((obj.K*cal_al_state(1:obj.nodeStateLength))(4:6));
|
||||
printf("<algo_init> validating...\n");
|
||||
for i = 1:length(ang)
|
||||
if sum( (abs(_d(i) - d(i,3))) < .1 )
|
||||
printf("<algo_init> %i pass\n",i);
|
||||
else
|
||||
printf("<algo_init> inverte on %i\n",i);
|
||||
ang(i) = 180 + fDeggreCosine(save_state(1),d(i,2),d(i,1));
|
||||
endif
|
||||
endfor
|
||||
cal_al_state = [al_state(1:3);
|
||||
d(1,2)*cosd(ang(1))+save_state(1);
|
||||
d(1,2)*sind(ang(1))+save_state(2);
|
||||
0 ;
|
||||
d(2,2)*cosd(ang(2))+save_state(1);
|
||||
d(2,2)*sind(ang(2))+save_state(2);
|
||||
0]
|
||||
if norm(cal_al_state - st(1:9)) < .1
|
||||
printf("<algo_init> done\n");
|
||||
algo=3;
|
||||
else
|
||||
printf("<algo_init> fail\n");
|
||||
algo=9;
|
||||
endif
|
||||
else
|
||||
al_state = obj.param.model.A * al_state ...
|
||||
+ obj.param.model.B ...
|
||||
* arrayfun(obj.param.model.limit_motor, ...
|
||||
(obj.param.model.Nst * set_point ...
|
||||
- obj.param.model.Kst*al_state ));
|
||||
state(1:3) = al_state(1:3);
|
||||
state(obj.nodeStateLength+1:obj.nodeStateLength+3) = al_state(4:6);
|
||||
endif
|
||||
case 3;
|
||||
state = obj.A(st)*st + obj.B(st)*obj.d + obj.Kb*vRef;
|
||||
case 9;
|
||||
state = obj.Av2(st,[0 0 0])*st + obj.B(st)*obj.d + obj.Kb*vRef;;
|
||||
endswitch;
|
||||
save "algo_info.m" ang save_state algo d;
|
||||
endfunction
|
||||
|
|
@ -1,5 +1,13 @@
|
|||
function obj = formationControl2ndBuilder(edgeL,robStateLen,kp1,kp2,ki1,ki2,dScale,nNodeVref)
|
||||
function obj = formationControl2ndBuilder(param)
|
||||
addpath("./networks-toolbox");
|
||||
edgeL = param.edgeL;
|
||||
model = param.model.origin;
|
||||
kp1 = param.kp1;
|
||||
kp2 = param.kp2;
|
||||
ki1 = param.ki1;
|
||||
ki2 = param.ki2;
|
||||
dScale = param.dScale;
|
||||
nNodeVref = param.nNodeVref;
|
||||
obj = struct();
|
||||
[obj.R, obj.K, obj.d] = rigidityMatrixFnc(edgeL,robStateLen);
|
||||
obj.m = length(obj.d);
|
||||
|
|
|
|||
|
|
@ -1,5 +1,13 @@
|
|||
function obj = formationControl1stModelBuilder(edgeL,model,kp1,kp2,ki1,ki2,dScale,nNodeVref)
|
||||
function obj = formationControl1stModelBuilder(param)
|
||||
addpath("./networks-toolbox");
|
||||
edgeL = param.edgeL;
|
||||
model = param.model.origin;
|
||||
kp1 = param.kp1;
|
||||
kp2 = param.kp2;
|
||||
ki1 = param.ki1;
|
||||
ki2 = param.ki2;
|
||||
dScale = param.dScale;
|
||||
nNodeVref = param.nNodeVref;
|
||||
obj = struct();
|
||||
robStateLen = size(model.A,1)
|
||||
[obj.R, obj.K, obj.d] = rigidityMatrixFnc(edgeL,robStateLen);
|
||||
|
|
@ -13,7 +21,7 @@ function obj = formationControl1stModelBuilder(edgeL,model,kp1,kp2,ki1,ki2,dScal
|
|||
% pilih robot mana yang ingin dijalankan dengan mengganti 0 menjadi 1
|
||||
% r 1 2 3 r 1 2 3
|
||||
% v v v v v v
|
||||
obj.A = kron([zeros(obj.n) zeros(obj.n);zeros(obj.n) diag([1 1 1])],model.A) + kron([zeros(obj.n) diag([1 1 1]);zeros(obj.n) zeros(obj.n)],eye(obj.n));
|
||||
obj.A = kron([zeros(obj.n) zeros(obj.n);zeros(obj.n) diag([1 0 0])],model.A) + kron([zeros(obj.n) diag([1 0 0]);zeros(obj.n) zeros(obj.n)],eye(obj.n));
|
||||
obj.B = kron([zeros(obj.n); eye(obj.n)],model.B);
|
||||
obj.Ksgn = kron([zeros(obj.n) zeros(obj.n);zeros(obj.n) eye(obj.n)],model.K);
|
||||
obj.controller = @(set_point,st) arrayfun(model.limit_motor,_nst * set_point + _kst * st(1:(2 * obj.nodeStateLength)));
|
||||
|
|
|
|||
|
|
@ -1,5 +1,13 @@
|
|||
function obj = formationControl2ndBuilder(edgeL,robStateLen,kp1,kp2,ki1,ki2,dScale,nNodeVref)
|
||||
function obj = formationControl2ndBuilder(param)
|
||||
addpath("./networks-toolbox");
|
||||
edgeL = param.edgeL;
|
||||
model = param.model.origin;
|
||||
kp1 = param.kp1;
|
||||
kp2 = param.kp2;
|
||||
ki1 = param.ki1;
|
||||
ki2 = param.ki2;
|
||||
dScale = param.dScale;
|
||||
nNodeVref = param.nNodeVref;
|
||||
obj = struct();
|
||||
[obj.R, obj.K, obj.d] = rigidityMatrixFnc(edgeL,robStateLen);
|
||||
obj.m = length(obj.d);
|
||||
|
|
|
|||
|
|
@ -1,7 +1,16 @@
|
|||
function obj = formationControl2ndModelBuilder(edgeL,model,kp1,kp2,ki1,ki2,dScale,nNodeVref)
|
||||
function obj = formationControl2ndModelBuilder(param)
|
||||
addpath("./networks-toolbox");
|
||||
edgeL = param.edgeL;
|
||||
model = param.model.origin;
|
||||
kp1 = param.kp1;
|
||||
kp2 = param.kp2;
|
||||
ki1 = param.ki1;
|
||||
ki2 = param.ki2;
|
||||
dScale = param.dScale;
|
||||
nNodeVref = param.nNodeVref;
|
||||
% pkg load parallel
|
||||
obj = struct();
|
||||
obj.param = param;
|
||||
robStateLen = size(model.syscl.A,1)
|
||||
[obj.R, obj.K, obj.d] = rigidityMatrixFnc(edgeL,robStateLen);
|
||||
obj.m = length(obj.d);
|
||||
|
|
@ -10,14 +19,48 @@ function obj = formationControl2ndModelBuilder(edgeL,model,kp1,kp2,ki1,ki2,dScal
|
|||
obj.stateLength = 4 * obj.nodeStateLength + obj.m;
|
||||
|
||||
% build matrix sistem
|
||||
obj.A1 = @(st) [zeros(obj.nodeStateLength), zeros(obj.nodeStateLength), zeros(obj.nodeStateLength),zeros(obj.nodeStateLength,obj.m), eye(obj.nodeStateLength)];
|
||||
obj.A2 = @(st) [-kp2*obj.R(st(1:(obj.nodeStateLength)),obj.K)'*obj.R(st(1:(obj.nodeStateLength)),obj.K), -kp1*eye(obj.nodeStateLength), -eye(obj.nodeStateLength), -obj.R(st(1:(obj.nodeStateLength)),obj.K)',zeros(obj.nodeStateLength)];
|
||||
obj.A3 = @(st) [zeros(obj.nodeStateLength), ki1*eye(obj.nodeStateLength), zeros(obj.nodeStateLength), zeros(obj.nodeStateLength,obj.m), zeros(obj.nodeStateLength)];
|
||||
obj.A4 = @(st) [ki2*obj.R(st(1:(obj.nodeStateLength)),obj.K), zeros(obj.m,obj.nodeStateLength), zeros(obj.m,obj.nodeStateLength), zeros(obj.m), zeros(obj.m,obj.nodeStateLength)];
|
||||
obj.A1 = @(st) [zeros(obj.nodeStateLength), ...
|
||||
zeros(obj.nodeStateLength), ...
|
||||
zeros(obj.nodeStateLength), ...
|
||||
zeros(obj.nodeStateLength,obj.m), ...
|
||||
eye(obj.nodeStateLength) ...
|
||||
];
|
||||
obj.A2 = @(st) [-kp2*obj.R(st(1:(obj.nodeStateLength)), ...
|
||||
obj.K)'*obj.R(st(1:(obj.nodeStateLength)), ...
|
||||
obj.K), -kp1*eye(obj.nodeStateLength), ...
|
||||
-eye(obj.nodeStateLength), ...
|
||||
-obj.R(st(1:(obj.nodeStateLength)),obj.K)', ...
|
||||
zeros(obj.nodeStateLength) ...
|
||||
];
|
||||
obj.A3 = @(st) [zeros(obj.nodeStateLength), ...
|
||||
ki1*eye(obj.nodeStateLength), ...
|
||||
zeros(obj.nodeStateLength), ...
|
||||
zeros(obj.nodeStateLength,obj.m), ...
|
||||
zeros(obj.nodeStateLength) ...
|
||||
];
|
||||
obj.A4 = @(st) [ki2*obj.R(st(1:(obj.nodeStateLength)), ...
|
||||
obj.K), zeros(obj.m,obj.nodeStateLength), ...
|
||||
zeros(obj.m,obj.nodeStateLength), ...
|
||||
zeros(obj.m), ...
|
||||
zeros(obj.m,obj.nodeStateLength) ...
|
||||
];
|
||||
obj.MOsgn =@(st) diag([model.sgn(st(1)) model.sgn(st(2)) model.sgn(st(3))]);
|
||||
obj.MOdist = @(st) [obj.MOsgn(st(31:33)) zeros(robStateLen) zeros(robStateLen); zeros(robStateLen) obj.MOsgn(st(34:36)) zeros(robStateLen); zeros(robStateLen) zeros(robStateLen) obj.MOsgn(st(37:39))];
|
||||
obj.MO = @(st) [zeros(obj.nodeStateLength), kron(eye(robStateLen),model.syscl.B) , zeros(obj.nodeStateLength), zeros(obj.nodeStateLength,obj.m), kron(eye(robStateLen),model.syscl.A)+obj.MOdist(st)];
|
||||
obj.MOdist = @(st) [obj.MOsgn(st(31:33)) zeros(robStateLen) zeros(robStateLen); ...
|
||||
zeros(robStateLen) obj.MOsgn(st(34:36)) zeros(robStateLen); ...
|
||||
zeros(robStateLen) zeros(robStateLen) obj.MOsgn(st(37:39)) ...
|
||||
];
|
||||
obj.MO = @(st) [zeros(obj.nodeStateLength), ...
|
||||
kron(diag([1 1 1]),model.syscl.B), ...
|
||||
zeros(obj.nodeStateLength), zeros(obj.nodeStateLength,obj.m), ...
|
||||
kron(eye(robStateLen),model.syscl.A)+obj.MOdist(st) ...
|
||||
];
|
||||
obj.MOv2 = @(st,select) [zeros(obj.nodeStateLength), ...
|
||||
kron(diag(select),model.syscl.B), ...
|
||||
zeros(obj.nodeStateLength), zeros(obj.nodeStateLength,obj.m), ...
|
||||
kron(eye(robStateLen),model.syscl.A)+obj.MOdist(st) ...
|
||||
];
|
||||
obj.A = @(st) [obj.A1(st); obj.A2(st); obj.A3(st); obj.A4(st); obj.MO(st)];
|
||||
obj.Av2 = @(st,select) [obj.A1(st); obj.A2(st); obj.A3(st); obj.A4(st); obj.MOv2(st,select)];
|
||||
obj.sizeA = size(obj.A(ones(obj.stateLength,1)));
|
||||
|
||||
% build matrix input
|
||||
|
|
@ -30,7 +73,7 @@ function obj = formationControl2ndModelBuilder(edgeL,model,kp1,kp2,ki1,ki2,dScal
|
|||
obj.Kb = [kron(obj.Kb,eye(robStateLen)); zeros(obj.m,robStateLen); zeros(obj.nodeStateLength,robStateLen)];
|
||||
obj.d = obj.d*dScale;
|
||||
obj.dss = @(st,h,vRef) (eye(obj.sizeA) + (obj.A(st)*h))*st + obj.B(st)*h*obj.d + obj.Kb*h*vRef;
|
||||
obj.ss = @(st,vRef) obj.A(st)*st + obj.B(st)*obj.d + obj.Kb*vRef;
|
||||
obj.ss= @(st,vRef) obj.A(st)*st + obj.B(st)*obj.d + obj.Kb*vRef;
|
||||
|
||||
printf("============== Formation Control ============ \n");
|
||||
printf(" state model : %i \n", obj.nodeStateLength);
|
||||
|
|
|
|||
Binary file not shown.
File diff suppressed because it is too large
Load Diff
Loading…
Reference in New Issue