add plot move

SOURCE/SM_2015_Rozenheck_PLOT_MOVE.m

@@ -0,0 +1,129 @@
+clear -all
+
+graphics_toolkit('gnuplot')
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% config nang kene
+
+bParam = struct();
+bParam.model = omnidirectional(0.94, 0.96, 0.01, 2.2, 1.5,0.099,0.1,1.5,0.025,30,0.035,19,0.00011,1.69,0.0059,0.0059,0.9,9.1,6,'./');
+bParam.edgeL = [1 2 1;3 2 1;3 1 1;];
+bParam.dScale = 3;
+bParam.nNodeVref = 1;
+bParam.ki1 = 0;
+bParam.ki2 = 0;  
+bparam.hStep = .01;
+bParam.tspan = 1:bparam.hStep:20;
+# coba solo
+bParam.corRobot = [0; 0;0;1; 2;0;3; -3;0;]; % xy xy xy
+  ## CASE 1
+  bParam.kp1 = 50;
+  bParam.kp2 = 3;
+
+  bP(1,1) = bParam;
+# solo coba 
+###### KOOR 1
+##bParam.corRobot = [0; 0;0;1; 2;0;-2; 3;0;]; % xy xy xy
+##  ## CASE 1
+##  bParam.kp1 = 50;
+##  bParam.kp2 = 3;
+##
+##  bP(1,1) = bParam;
+##        
+##  ## CASE 2
+##  bParam.kp1 = 80;
+##  bParam.kp2 = 7;
+##
+##  bP(1,2) = bParam;
+##
+##  # CASE 3
+##  bParam.kp1 = 100;
+##  bParam.kp2 = 15;
+##
+##  bP(1,3) = bParam;
+##
+#### KOOR 2
+##bParam.corRobot = [0; 0;0;-2; -4;0;3; -2;0;]; % xy xy xy
+##  ## CASE 1
+##  bParam.kp1 = 50;
+##  bParam.kp2 = 3;
+##
+##  bP(2,1) = bParam;
+##        
+##  ## CASE 2
+##  bParam.kp1 = 80;
+##  bParam.kp2 = 7;
+##
+##  bP(2,2) = bParam;
+##
+##  # CASE 3
+##  bParam.kp1 = 100;
+##  bParam.kp2 = 15;
+##
+##  bP(2,3) = bParam;
+##
+## KOOR 3
+##bParam.corRobot = [0; 0;0;1; 2;0;3; -3;0;]; % xy xy xy
+##  ## CASE 1
+##  bParam.kp1 = 50;
+##  bParam.kp2 = 3;
+##
+##  bP(3,1) = bParam;
+##        
+##  ## CASE 2
+##  bParam.kp1 = 80;
+##  bParam.kp2 = 7;
+##
+##  bP(3,2) = bParam;
+##
+##  # CASE 3
+##  bParam.kp1 = 100;
+##  bParam.kp2 = 15;
+##
+##  bP(3,3) = bParam;
+##  
+#### KOOR 4
+##bParam.corRobot = [0; 0;0;-2; -3;0;3; 2;0;]; % xy xy xy  
+##  ## CASE 1
+##  bParam.kp1 = 50;
+##  bParam.kp2 = 3;
+##
+##  bP(4,1) = bParam;
+##        
+##  ## CASE 2
+##  bParam.kp1 = 80;
+##  bParam.kp2 = 7;
+##
+##  bP(4,2) = bParam;
+##
+##  # CASE 3
+##  bParam.kp1 = 100;
+##  bParam.kp2 = 15;
+##
+##  bP(4,3) = bParam;
+  
+close all
+
+for i = 1:size(bP,1)
+  [t,y,Dd,DdNorm] = run_my_simulation(bP(i,1));
+  
+  plot_move(y', length(bP(1,1).tspan), "/home/a2nr/Documents/FormationControlSimulation/SOURCE/output/",bP(i,1));
+  
+  figure
+  plot_con( y', bParam.edgeL,  [1, length(bP(1,1).tspan)] ,bP(i,1));
+  title(sprintf("Motion dari Robot Case %i ", i ));
+  
+##  figure
+##  resdd = struct();
+##  resdd.DdNorm = DdNorm;
+##  allresdd(1) = resdd;
+##  for j = 2:size(bP,2)
+##    [t,y,Dd,resdd.DdNorm] = run_my_simulation(bP(i,j));
+##    allresdd(j) = resdd;
+##  endfor
+##  plot(t,allresdd(1).DdNorm,t,allresdd(2).DdNorm,t,allresdd(3).DdNorm);
+##  title(sprintf("Norm Distance error Case %i ", i));
+endfor
+
+
+
+
+

SOURCE/plot_move.m

@@ -0,0 +1,50 @@
+function plot_move(yOut,time,path,bParam)
+  lengthCorRobot = length(bParam.corRobot);
+  robStateLen = size(bParam.model.origin.A,1);
+  lengthNode = length(bParam.edgeL);
+  tspan = bParam.tspan;
+
+  xi = 1:robStateLen:lengthCorRobot-1;
+  yi = 2:robStateLen:lengthCorRobot;
+  str_tmp = "@(e) plot(";
+  for i = 1:lengthNode-1
+    str_tmp = strcat(str_tmp,sprintf("yOut(%i,1:e),yOut(%i,1:e),",xi(i),yi(i)));
+  endfor
+  str_tmp = strcat(str_tmp,sprintf("yOut(%i,1:e),yOut(%i,1:e));",xi(i+1),yi(i+1)));
+  _plot_move = eval(str_tmp);     %_plot_move(end);
+  
+  
+                                  % membuat fungsi plot robot di waktu
+                                  % tertentu
+  str_tmp = "@(t) plot( [";
+  for i = 1:robStateLen:lengthCorRobot
+    str_tmp = strcat( str_tmp, sprintf("yOut(%i,t), ",i));
+  endfor
+  str_tmp = strcat( str_tmp, sprintf("], ["));
+  for i = 2:robStateLen:lengthCorRobot
+    str_tmp = strcat( str_tmp, sprintf("yOut(%i,t), ",i));
+  endfor
+  str_tmp = strcat( str_tmp, sprintf("], \"^\");"));
+  plot_rb = eval(str_tmp);         % plot_rb(coordinat)
+  
+  str_tmp = "legend(";
+  for i = 1:numNodes(edgeL2adjL(bParam.edgeL))-1;
+    str_tmp =strcat(str_tmp,sprintf("\"R%i \", ",i));
+  endfor
+  str_tmp =strcat(str_tmp,sprintf("\"R%i \" )",++i)); 
+  
+  
+  for t=1:time
+    f=figure('visible', 'off');
+    hold on
+      _plot_move(t);
+      plot_rb(t);
+      eval(str_tmp);
+      filename=strcat(path,sprintf('x%03d%03d_y%03d%03d_kp1%03d_kp2%03d_%05d.png', ...
+                      bParam.corRobot(4),bParam.corRobot(5),bParam.corRobot(7),bParam.corRobot(8),...
+                      bParam.kp1,bParam.kp2,t));
+      print(filename);
+    hold off
+    close(f);
+  endfor
+endfunction