support robot 3 item state
a2nr
parent
64e56ec5d8
commit
bc1085f9eb
|
|
@ -1,2 +1,4 @@
|
|||
*.data
|
||||
*.csv
|
||||
*.png
|
||||
/SOURCE/octave-workspace
|
||||
|
|
|
|||
Binary file not shown.
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
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);
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
Loading…
Reference in New Issue