a2nr
/
FormationControlSimulation
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

support robot 3 item state

master
a2nr 2020-07-29 11:31:38 +07:00
parent 64e56ec5d8
commit bc1085f9eb
6 changed files with 48 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.gitignore vendored
View File

@ -1,2 +1,4 @@
*.data
*.csv
*.png
/SOURCE/octave-workspace

BIN
DOC/Oh_et_al-2014-International_Journal_of_Robust_and_Nonlinear_Control.pdf
View File

Binary file not shown.

40
SOURCE/SM_2015_Rozenheck.m
View File

@ -10,10 +10,11 @@ dRobot = [ 1;
1;
1;
];
corRobot = [1.5; 1.7;
2; 2.5;
2.5; 2.8;
corRobot = [1.5; 1.7;0;
2; 2.5;0;
2.5; 2.8;0;
]; % xy xy xy
robStateLen = length(corRobot)/length(dRobot)
B = [
1 0; 0 1;
0 0; 0 0;
@ -21,17 +22,17 @@ B = [
];
kp = 80;
ki = 1;
% fvref = fncSpeedRef('ysin',50,2);
% fvref = fncSpeedRef('xsin',50,2);
% fvref = fncSpeedRef('cw',-300,1);
% fvref = fncSpeedRef('s',50,50);
fvref = fncSpeedRef('s',0,0);
% fvref = fncSpeedRef('ysin',50,2,robStateLen);
% fvref = fncSpeedRef('xsin',50,2,robStateLen);
% fvref = fncSpeedRef('cw',-300,1,robStateLen);
% fvref = fncSpeedRef('s',50,50,robStateLen);
fvref = fncSpeedRef('s',0,0,robStateLen);
h = .1;
tspan = 1:h:10;
sys = formationControl2ndBuilder(conRobot,kp,kp,ki,ki,length_d,1);
sys = formationControl2ndBuilder(conRobot,robStateLen,kp,kp,ki,ki,length_d,1);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% config nang nduwur
[R,K,d] = rigidityMatrixFnc(conRobot);
[R,K,d] = rigidityMatrixFnc(conRobot,robStateLen);
_zero = zeros(size(R(corRobot,K),1),1);
sInit = [corRobot; _zero;];
sAnsInit =[_zero; _zero;];
@ -59,28 +60,33 @@ figure(1)
% plot trayektori dari setiap robot
str_tmp = "plot(";
for i = 1:length(corRobot)-1
str_tmp = strcat(str_tmp,sprintf("y'(%i,:),",i));
xi = 1:robStateLen:length(corRobot)-1
yi = 2:robStateLen:length(corRobot)
for i = 1:sys.n-1
str_tmp = strcat(str_tmp,sprintf("y'(%i,:),y'(%i,:),",xi(i),yi(i)));
endfor
str_tmp = strcat(str_tmp,sprintf("y'(%i,:));",i+1));
str_tmp = strcat(str_tmp,sprintf("y'(%i,:),y'(%i,:));",xi(i+1),yi(i+1)));
eval(str_tmp);
hold on
% membuat fungsi plot robot di waktu
% tertentu
str_tmp = "@(t) plot( [";
for i = 1:2:length(corRobot)
for i = 1:robStateLen:length(corRobot)
str_tmp = strcat( str_tmp, sprintf("y'(%i,t), ",i));
endfor
str_tmp = strcat( str_tmp, sprintf("], ["));
for i = 2:2:length(corRobot)
for i = 2:robStateLen:length(corRobot)
str_tmp = strcat( str_tmp, sprintf("y'(%i,t), ",i));
endfor
str_tmp = strcat( str_tmp, sprintf("], \"^\");"));
plot_rb = eval(str_tmp);
xrb = 1:2:length(corRobot);
yrb = 2:2:length(corRobot);
xrb = 1:robStateLen:length(corRobot);
yrb = 2:robStateLen:length(corRobot);
% fungsi untuk plot coneksi di waktu
% tertentu

11
SOURCE/fncSpeedRef.m
View File

@ -1,4 +1,4 @@
function fncSr = fncSpeedRef(param,r,v)
function fncSr = fncSpeedRef(param,r,v,robStateLen)
sp = [1, 2]; % 1 = x; 2 = y
if (length(param) == 4)
% 'xsin' / 'xcos'
@ -30,8 +30,13 @@ function fncSr = fncSpeedRef(param,r,v)
else
printf("Karepmu opo ? \n\n bukak fungsi speedRef sek diwoco komen e \n\n");
endif
strAll = strcat("fncSr = @(t) [",strSfx, ";\ ", strSfy, "];");
if (robStateLen==2)
strAll = strcat("fncSr = @(t) [",strSfx, ";\ ", strSfy, "];");
elseif(robStateLen==3)
strAll = strcat("fncSr = @(t) [",strSfx, ";\ ", strSfy, ";\ 0];");
else
printf("ERROR REFSPEED NOT SUPPORT!!");
endif
%% printf(strAll)
eval(strAll);

19
SOURCE/formationControl2ndBuilder.m
View File

@ -1,27 +1,28 @@
function obj = formationControl2ndBuilder(edgeL,kp1,kp2,ki1,ki2,dScale,nNodeVref)
function obj = formationControl2ndBuilder(edgeL,robStateLen,kp1,kp2,ki1,ki2,dScale,nNodeVref)
addpath("./networks-toolbox");
obj = struct();
[obj.R, obj.K, obj.d] = rigidityMatrixFnc(edgeL);
[obj.R, obj.K, obj.d] = rigidityMatrixFnc(edgeL,robStateLen);
obj.m = length(obj.d);
obj.n = numNodes(edgeL);
obj.stateLength = 3 * 2*obj.n + obj.m;
obj.nodeStateLength = robStateLen*obj.n;
obj.stateLength = 3 * obj.nodeStateLength + obj.m;
% build matrix sistem
obj.A1 = @(st) [zeros(2*obj.n), eye(2*obj.n), zeros(2*obj.n), zeros(obj.n*2,obj.m)];
obj.A2 = @(st) [-kp2*obj.R(st(1:(2*obj.n)),obj.K)'*obj.R(st(1:(2*obj.n)),obj.K), -kp1*eye(2*obj.n), -eye(2*obj.n), -obj.R(st(1:(2*obj.n)),obj.K)'];
obj.A3 = @(st) [zeros(2*obj.n), ki1*eye(2*obj.n), zeros(2*obj.n), zeros(obj.n*2,obj.m)];
obj.A4 = @(st) [ki2*obj.R(st(1:(2*obj.n)),obj.K), zeros(obj.m,obj.n*2), zeros(obj.m,obj.n*2), zeros(obj.m)];
obj.A1 = @(st) [zeros(obj.nodeStateLength), eye(obj.nodeStateLength), zeros(obj.nodeStateLength), zeros(obj.nodeStateLength,obj.m)];
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)'];
obj.A3 = @(st) [zeros(obj.nodeStateLength), ki1*eye(obj.nodeStateLength), zeros(obj.nodeStateLength), zeros(obj.nodeStateLength,obj.m)];
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)];
obj.A = @(st) [obj.A1(st); obj.A2(st); obj.A3(st); obj.A4(st)];
obj.sizeA = size(obj.A(ones(obj.stateLength,1)));
% build matrix input
obj.B = @(st) [zeros(2*obj.n,obj.m); kp2*obj.R(st(1:(2*obj.n)),obj.K)'; zeros(2*obj.n,obj.m); -ki2*eye(obj.m,obj.m)]
obj.B = @(st) [zeros(obj.nodeStateLength,obj.m); kp2*obj.R(st(1:(obj.nodeStateLength)),obj.K)'; zeros(obj.nodeStateLength,obj.m); -ki2*eye(obj.m,obj.m)]
obj.sizeB = size(obj.B(ones(obj.stateLength,1)));
%build state space
obj.Kb = zeros(obj.n*3,1);
obj.Kb(obj.n+nNodeVref) = 1;
obj.Kb = [kron(obj.Kb,eye(2)); zeros(obj.m,2)];
obj.Kb = [kron(obj.Kb,eye(robStateLen)); zeros(obj.m,robStateLen)];
obj.d = obj.d*dScale;
obj.ssDisc = @(st,vRef,h) (eye(obj.sizeA) + (obj.A(st)*h))*st + obj.B(st)*h*obj.d + obj.Kb*vRef;
obj.ss = @(st,vRef) obj.A(st)*st + obj.B(st)*obj.d + obj.Kb*vRef;

10
SOURCE/rigidityMatrixFnc.m
View File

@ -1,4 +1,4 @@
function [R,K, d] = rigidityMatrixFnc (edgeL)
function [R,K, d] = rigidityMatrixFnc (edgeL,stateLen)
%% Fungsi ini digunakan untuk menggenerasi fungsi rigidity R.
%% diaman fungsi tersebut adalah hasil turunan dari fungsi edge
addpath("./networks-toolbox");
@ -13,14 +13,14 @@ function [R,K, d] = rigidityMatrixFnc (edgeL)
% generasi vector error menggunakan matrix
% koneksi
tmp = getNodes(gInc,'adjacency');
K = kron(adj2inc(edgeL2adj(edgeL))',eye(2));
K = kron(adj2inc(edgeL2adj(edgeL))',eye(stateLen));
edgeVector = @(x,k) k*x;
% hasil turunan dari fungsi edge
str = "errBlockDiagonal = @(errVec) (blkdiag(";
for i = 1:2:(length(tmp)*2)-2
str = strcat(str,sprintf("errVec(%i:%i)', ",i,i+1));
for i = 1:stateLen:(length(tmp)*stateLen)-stateLen
str = strcat(str,sprintf("errVec(%i:%i)', ",i,i+(stateLen-1)));
endfor
str = strcat(str,sprintf("errVec(%i:%i)' ));",i+2,i+3));
str = strcat(str,sprintf("errVec(%i:%i)' ));",i+stateLen,i+(stateLen*2 -1)));
% return sebagai fungsi R
eval(str); %% replicate from https://www.mathworks.com/help/matlab/ref/blkdiag.html
R = @(x,k) errBlockDiagonal(edgeVector(x,k))*k;