Update support plot analisis

SOURCE/SM_2015_Rozenheck.m

@@ -1,6 +1,5 @@
 clear -all
 close all
-%% conRobot = [2 1 1; 3 2 1; 3 1 1;3 4 1; 2 4 1; 5 3 1; 5 1 1;6 4 1; 6 2 1; 5 6 1];
 conRobot = [1 2 1;
             1 4 1;
             1 5 1;
@@ -9,10 +8,31 @@ conRobot = [1 2 1;
             2 4 1;
             3 4 1;
             4 5 1;
-            5 6 1;];
-%% conRobot = [2 1 1; 3 2 1; 3 1 1;3 4 1; 2 4 1;];
-
-%% corRobot = [0; 0; 1; 0; 2; 2; 1; 1; -1; -1; 0; -1; ]; % xy xy xy
+            5 6 1;
+            5 2 1;
+            %% 6 3 1;
+            %% 6 4 1;
+            %% 6 2 1;
+            %% 3 1 1;
+            %% 3 5 1;
+           ];
+dRobot = [ 1;
+           1;
+           1;
+           1;
+           1;
+           1;
+           1;
+           1;
+           1;
+           1;
+           %% 1;
+           %% 2;
+           %% 2;
+           %% 2;
+           %% 2;
+           %% 2;
+];
 corRobot = [2; 2.5;
             2.7; 2.6;
             3; 2.3;
@@ -20,10 +40,8 @@ corRobot = [2; 2.5;
             1.8; 1.5;
             1.5; 2;
            ]; % xy xy xy
-%% corRobot = [0; 0; 1; 0; 2; 2; 1; 1;]; % xy xy xy
-d = 3;
 %% figure(1)
-[R,K] = rigidityMatrixFnc(conRobot);
+[R,K,d] = rigidityMatrixFnc(conRobot);
 vref = [-5; 5];
 B = [
      1 0; 0 1;
@@ -33,16 +51,30 @@ B = [
      0 0; 0 0;
      0 0; 0 0;
     ];
-kp = 2;
+kp = 10;
 ki = 3;
-
-sInit = [corRobot; zeros(size(R(corRobot,K),1),1)];
+_zero = zeros(size(R(corRobot,K),1),1);
+sInit = [corRobot; _zero;];
+sAnsInit =[_zero; _zero;];
 %% tspan = linspace(1,0.1,30);
-tspan = 1:0.01:5;
-dydt = @(t, y) systm_robot(t, y,d, R,K, kp, ki, B, vref);
-
+tspan = 1:0.01:10;
+dydt = @(t, y) systm_robot(t, y,(dRobot*6), R,K, kp, ki, B, vref);
 [t,y] = ode45(dydt, tspan, sInit);
-%% figure(2)
+
+cntr = 1;
+bypassCntr = @(c) cntr;
+plusPlusCntr = @(c) cntr++;
+
+dyansdt = @(t, yin) systm_anlys_robot(t, yin, y(:,1:(2*numNodes(edgeL2adj(conRobot)))) ,bypassCntr, plusPlusCntr ,(dRobot*6), R, K, kp, ki, B, vref);
+[t,yans] = ode45(dyansdt, tspan, sAnsInit);
+
+ddYans = zeros(size(yans,1),1);
+for i = 1:size(yans,1)
+  ddYans(i) = norm(yans(i,1:(size(yans,2)/2)));
+endfor
+
+figure(1)
+
 str_tmp = "plot(";
 for i = 1:length(corRobot)-1
   str_tmp = strcat(str_tmp,sprintf("y'(%i,:),",i));
@@ -76,6 +108,17 @@ endfunction
 plot_con(plot_rb, y', conRobot, xrb, yrb, 1);
 plot_con(plot_rb, y', conRobot, xrb, yrb, length(tspan));
 
+
+str_tmp = "legend(";
+for i = 1:numNodes(edgeL2adjL(conRobot))-1;
+  str_tmp =strcat(str_tmp,sprintf("\"Robot %i \", ",i));
+endfor
+str_tmp =strcat(str_tmp,sprintf("\"Robot %i \" )",++i));
+eval(str_tmp)
+
+figure(2)
+plot([1:length(ddYans)],ddYans )
+
 %% plot_rb(20);
 %% plot_rb(40);
 %% plot_rb(60);

SOURCE/rigidityMatrixFnc.m

@@ -1,10 +1,11 @@
-function [R,K] = rigidityMatrixFnc (edgeL)
+function [R,K, d] = rigidityMatrixFnc (edgeL)
   %% 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);
   gInc = adj2inc(tmp);
   %% drawCircGraph(tmp)
   tmp = getNodes(gInc,'adjacency')
@@ -21,4 +22,5 @@ function [R,K] = rigidityMatrixFnc (edgeL)
                                 % return sebagai fungsi R
   eval(str)
   R = @(x,k) errBlockDiagonal(edgeVector(x,k))*k;
+
 endfunction

SOURCE/systm_anlys_robot.m

@@ -0,0 +1,22 @@
+function dxdt = systm_anlys_robot(t,x,xRobot, C, ppC,d,R,K,kp,ki,B,vref)
+  n = length(K(1,:))/2;
+  m = length(K(:,1))/2;
+  %% l = length(R(ones(n*2,1),K));
+  dxdt = zeros(m*2,1);
+  x1 = x(1:m);
+  x2 = x(length(x1)+1:length(x));
+  counter = C(0);
+  %% length(x1)
+  %% length(x2)
+  %% if (t > 5) && (t < 15)
+  %% vref = [5; 10];
+  %% else
+  %% if (t > 15) && (t > 20)
+  %% vref = [5; -10];
+  %% endif
+  %% endif
+  %% size(xRobot)
+  dxdt(1:length(x1)) = -(2*kp*(R(xRobot(counter,:)',K)*R(xRobot(counter,:)',K)')*x1) -(2*(R(xRobot(counter,:)',K)*R(xRobot(counter,:)',K)')*x2) +((2*R(xRobot(counter,:)',K)*B)*vref);
+  dxdt(length(x1)+1:length(x)) =(ki*x1);
+  ppC();
+endfunction

SOURCE/systm_robot.m

@@ -1,6 +1,6 @@
 function dxdt = systm_robot(t,x,d,R,K,kp,ki,B,vref)
-  l = length(x);
   n = length(K(1,:))/2;
+  l = length(R(ones(n*2,1),K));
   m = length(K(:,1))/2;
   dxdt = zeros(l,1);
   x1 = x(1:n*2);
@@ -12,6 +12,6 @@ function dxdt = systm_robot(t,x,d,R,K,kp,ki,B,vref)
       %% vref = [5; -10];
     %% endif
   %% endif
-  dxdt(1:length(x1)) = -((kp*R(x1,K)'*R(x1,K))*x1)-(R(x1,K)'*x2)+((kp*R(x1,K)')*(ones(size(R(x1,K)',2),1)*d))+(B*vref);
-  dxdt(length(x1)+1:length(x)) = ki*R(x1,K)*x1-(ki*ones(m,1))*d;
+  dxdt(1:length(x1)) = -((kp*R(x1,K)'*R(x1,K))*x1)-(R(x1,K)'*x2)+((kp*R(x1,K)')*d)+(B*vref);
+  dxdt(length(x1)+1:length(x)) = ki*R(x1,K)*x1-(ki*d);
 endfunction