function obj = formationControl2ndBuilder(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;
    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