2021-03-02 08:24:32 +07:00
|
|
|
function obj = formationControl2ndBuilder(param)
|
2021-01-29 10:29:16 +07:00
|
|
|
addpath("./networks-toolbox");
|
2021-03-02 08:24:32 +07:00
|
|
|
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;
|
2021-01-29 10:29:16 +07:00
|
|
|
obj = struct();
|
|
|
|
|
[obj.R, obj.K, obj.d] = rigidityMatrixFnc(edgeL,robStateLen);
|
|
|
|
|
obj.m = length(obj.d);
|
|
|
|
|
obj.d = obj.d *dScale;
|
|
|
|
|
obj.n = numNodes(edgeL);
|
|
|
|
|
obj.nodeStateLength = robStateLen*obj.n;
|
|
|
|
|
obj.stateLength = 3 * obj.nodeStateLength + obj.m;
|
|
|
|
|
|
|
|
|
|
%build matrix system
|
|
|
|
|
obj.A = @(st) [-kp1*obj.R(st(1:(obj.nodeStateLength)),obj.K)'*obj.R(st(1:(obj.nodeStateLength)),obj.K) obj.R(st(1:(obj.nodeStateLength)),obj.K)'; ki1*obj.R(st(1:(obj.nodeStateLength)),obj.K) zeros(obj.m, obj.n)];
|
|
|
|
|
obj.sizeA = size(obj.A(ones(obj.stateLength,1)));
|
|
|
|
|
|
|
|
|
|
obj.B = @(st) [kp1*obj.R(st(1:(obj.nodeStateLength)),obj.K)'; -ki1*ones(obj.m)];
|
|
|
|
|
obj.sizeB = size(obj.B(ones(obj.stateLength,1)));
|
|
|
|
|
|
|
|
|
|
obj.dss = @(st,h,vRef) (eye(obj.sizeA) + (obj.A(st)*h))*st + (obj.B(st)*h)*obj.d ;
|
|
|
|
|
obj.ss = @(st,vRef) obj.A(st)*st + obj.B(st)*obj.d ;
|
|
|
|
|
|
|
|
|
|
printf("============== Formation Control ============ \n");
|
|
|
|
|
printf(" state model : %i \n", obj.nodeStateLength);
|
|
|
|
|
printf(" Node : %i \n", obj.n);
|
|
|
|
|
printf(" Edge : %i \n", obj.m);
|
|
|
|
|
printf("Dimention State : %i \n", obj.stateLength);
|
|
|
|
|
printf(" Dimention A : %i x %i \n", obj.sizeA);
|
|
|
|
|
printf(" Dimention B : %i x %i \n", obj.sizeB);
|
|
|
|
|
printf("============================================= \n");
|
|
|
|
|
|
|
|
|
|
endfunction
|
