100 lines
4.5 KiB
Matlab
100 lines
4.5 KiB
Matlab
function obj = formationControl2ndModelBuilder(param)
|
|
addpath("./networks-toolbox");
|
|
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;
|
|
% pkg load parallel
|
|
obj = struct();
|
|
obj.param = param;
|
|
robStateLen = size(model.syscl.A,1)
|
|
[obj.R, obj.K, obj.d] = rigidityMatrixFnc(edgeL,robStateLen);
|
|
obj.m = length(obj.d);
|
|
obj.n = numNodes(edgeL);
|
|
obj.nodeStateLength = robStateLen*obj.n;
|
|
obj.stateLength = 4 * obj.nodeStateLength + obj.m;
|
|
|
|
% build matrix sistem
|
|
obj.A1 = @(st) [zeros(obj.nodeStateLength), ...
|
|
zeros(obj.nodeStateLength), ...
|
|
zeros(obj.nodeStateLength), ...
|
|
zeros(obj.nodeStateLength,obj.m), ...
|
|
eye(obj.nodeStateLength) ...
|
|
];
|
|
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)', ...
|
|
zeros(obj.nodeStateLength) ...
|
|
];
|
|
obj.A3 = @(st) [zeros(obj.nodeStateLength), ...
|
|
ki1*eye(obj.nodeStateLength), ...
|
|
zeros(obj.nodeStateLength), ...
|
|
zeros(obj.nodeStateLength,obj.m), ...
|
|
zeros(obj.nodeStateLength) ...
|
|
];
|
|
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), ...
|
|
zeros(obj.m,obj.nodeStateLength) ...
|
|
];
|
|
obj.MOsgn =@(st) diag([model.sgn(st(1)) model.sgn(st(2)) model.sgn(st(3))]);
|
|
obj.MOdist = @(st) [obj.MOsgn(st(31:33)) zeros(robStateLen) zeros(robStateLen); ...
|
|
zeros(robStateLen) obj.MOsgn(st(34:36)) zeros(robStateLen); ...
|
|
zeros(robStateLen) zeros(robStateLen) obj.MOsgn(st(37:39)) ...
|
|
];
|
|
obj.MO = @(st) [zeros(obj.nodeStateLength), ...
|
|
kron(diag([1 1 1]),model.syscl.B), ...
|
|
zeros(obj.nodeStateLength), zeros(obj.nodeStateLength,obj.m), ...
|
|
kron(eye(robStateLen),model.syscl.A)+obj.MOdist(st) ...
|
|
];
|
|
obj.MOv2 = @(st,select) [zeros(obj.nodeStateLength), ...
|
|
kron(diag(select),model.syscl.B), ...
|
|
zeros(obj.nodeStateLength), zeros(obj.nodeStateLength,obj.m), ...
|
|
kron(eye(robStateLen),model.syscl.A)+obj.MOdist(st) ...
|
|
];
|
|
obj.A = @(st) [obj.A1(st); obj.A2(st); obj.A3(st); obj.A4(st); obj.MO(st)];
|
|
obj.Av2 = @(st,select) [obj.A1(st); ...
|
|
obj.A2(st); ...
|
|
obj.A3(st); ...
|
|
obj.A4(st); ...
|
|
obj.MOv2(st,select)];
|
|
obj.sizeA = size(obj.A(ones(obj.stateLength,1)));
|
|
|
|
% build matrix input
|
|
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); ...
|
|
zeros(obj.nodeStateLength,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(robStateLen)); ...
|
|
zeros(obj.m,robStateLen); ...
|
|
zeros(obj.nodeStateLength,robStateLen)];
|
|
obj.d = obj.d*dScale;
|
|
obj.dss = @(st,h,vRef) (eye(obj.sizeA) ...
|
|
+ (obj.A(st)*h))*st ...
|
|
+ obj.B(st)*h*obj.d ...
|
|
+ obj.Kb*h*vRef;
|
|
obj.ss= @(st,vRef) obj.A(st)*st + obj.B(st)*obj.d + obj.Kb*vRef;
|
|
|
|
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 |