30 lines
1.3 KiB
Matlab
30 lines
1.3 KiB
Matlab
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");
|
|
pkg load symbolic
|
|
% generasi koneksi bentuk incidence
|
|
tmp = edgeL2adj(edgeL);
|
|
d = ones(numEdges(tmp),1);
|
|
printf("Jumlah Edge : %i \n", length(d));
|
|
gInc = adj2inc(tmp);
|
|
tmp = numNodes(adj2adjL(tmp));
|
|
printf("Jumlah Robot : %i \n", tmp);
|
|
% generasi vector error menggunakan matrix
|
|
% koneksi
|
|
% tmp = getNodes(gInc,'adjacency');
|
|
K = kron(adj2inc(edgeL2adj(edgeL))',eye(stateLen));
|
|
tmp = 1:(size(K,1)/stateLen);
|
|
edgeVector = @(x,k) k*x;
|
|
% hasil turunan dari fungsi edge
|
|
str = "errBlockDiagonal = @(errVec) (blkdiag(";
|
|
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+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;
|
|
|
|
endfunction
|