diff --git a/.gitignore b/.gitignore index 1f75dc3..ecc0fa8 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,4 @@ *.data *.csv +*.png +/SOURCE/octave-workspace diff --git a/DOC/Oh_et_al-2014-International_Journal_of_Robust_and_Nonlinear_Control.pdf b/DOC/Oh_et_al-2014-International_Journal_of_Robust_and_Nonlinear_Control.pdf index 60d73be..ede13bb 100644 Binary files a/DOC/Oh_et_al-2014-International_Journal_of_Robust_and_Nonlinear_Control.pdf and b/DOC/Oh_et_al-2014-International_Journal_of_Robust_and_Nonlinear_Control.pdf differ diff --git a/SOURCE/SM_2015_Rozenheck.m b/SOURCE/SM_2015_Rozenheck.m index 29ae292..ed74260 100644 --- a/SOURCE/SM_2015_Rozenheck.m +++ b/SOURCE/SM_2015_Rozenheck.m @@ -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 diff --git a/SOURCE/fncSpeedRef.m b/SOURCE/fncSpeedRef.m index a67734d..72d0f51 100644 --- a/SOURCE/fncSpeedRef.m +++ b/SOURCE/fncSpeedRef.m @@ -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); diff --git a/SOURCE/formationControl2ndBuilder.m b/SOURCE/formationControl2ndBuilder.m index 51d75c1..08f62e4 100644 --- a/SOURCE/formationControl2ndBuilder.m +++ b/SOURCE/formationControl2ndBuilder.m @@ -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; diff --git a/SOURCE/rigidityMatrixFnc.m b/SOURCE/rigidityMatrixFnc.m index 72270f5..fbbb834 100644 --- a/SOURCE/rigidityMatrixFnc.m +++ b/SOURCE/rigidityMatrixFnc.m @@ -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;