sorry gustavo

sw/simulator/scilab/q3d/q3d_diff_flatness.sci

@@ -0,0 +1,87 @@
+//
+// Differential Flatness
+//
+
+
+DF_OX    = 1;  // Flat output first  coordinate
+DF_OZ    = 2;  // Flat output second coordinate
+DF_OSIZE = 2;
+DF_ORANK = 5;  // Number of time derivative needed
+
+fo_g       = 9.81;
+fo_mass    = 0.25;
+fo_inertia = 0.0078;
+
+global fo_traj;
+
+// state from flat output
+function [state] = df_state_of_fo(fo)
+  state = zeros(FDM_SSIZE, 1);
+  state(FDM_SX)      = fo(1,1);
+  state(FDM_SZ)      = fo(2,1);
+  state(FDM_SXD)     = fo(1,2);
+  state(FDM_SZD)     = fo(2,2);
+  theta = -atan(fo(1,3), fo_g + fo(2,3));
+  state(FDM_STHETA)  = theta;
+  thetad = -((fo_g + fo(2,3))*fo(1,4) - fo(1,3)*fo(2,4)) / ...
+      ((fo_g + fo(2,3))^2+fo(1,3)^2);
+  state(FDM_STHETAD) = thetad;
+endfunction
+
+// control input from flat output
+function [inp] = df_input_of_fo(fo)
+
+  x2   = fo(1,3);				 
+  z2p1 = fo(2,3)+9.81;				 
+
+  u1 =  fo_mass * sqrt((x2)^2 + (z2p1)^2);
+  
+  x3   = fo(1,4);				 
+  z3   = fo(2,4);				 
+  x4   = fo(1,5);				 
+  z4   = fo(2,5);				 
+  a = x4*z2p1 - z4*x2; 				 
+  b = z2p1^2+x2^2;
+  c = 2 * (z2p1*z3 + x2*x3);
+  d = x3*z2p1-z3*x2;
+  u2 = -fo_inertia * ( a/b - c*d/b^2);
+ 
+  inp = [u1; u2];
+  
+endfunction
+
+
+// 
+function [fo] = df_get_traj_polynomial(Xin, Xout, Uin, Uout, time)
+
+  fo = zeros(DF_OSIZE*DF_ORANK, length(time));
+  // boundary conditions
+  fo(1,1)  = Xin(FDM_SX);
+  fo(2,1)  = Xin(FDM_SZ);
+  fo(3,1)  = Xin(FDM_SXD);
+  fo(4,1)  = Xin(FDM_SZD);
+  fo(5,1)  = -Uin(1)*sin(Xin(FDM_STHETA));
+  fo(6,1)  =  Uin(1)*(cos(Xin(FDM_STHETA)) - 1);
+  fo(7,1)  = 0;
+  fo(8,1)  = 0;
+  fo(9,1)  = 0;
+  fo(10,1) = 0;
+  
+  fo(1, length(time)) = Xout(FDM_SX);
+  fo(2, length(time)) = Xout(FDM_SZ);
+  fo(3, length(time)) = Xout(FDM_SXD);
+  fo(4, length(time)) = Xout(FDM_SZD);
+  fo(5, length(time)) = -Uout(1)*sin(Xout(FDM_STHETA));
+  fo(6, length(time)) =  Uout(1)*(cos(Xout(FDM_STHETA)) - 1);
+  fo(7, length(time)) = 0;
+  fo(8, length(time)) = 0;
+  fo(9, length(time)) = 0;
+  fo(10,length(time)) = 0;  
+
+ 
+endfunction
+
+
+
+
+

sw/simulator/scilab/q3d/q3d_polynomials.sci

@@ -0,0 +1,117 @@
+//
+// traj is a n_compo*n_order*n_sample vector
+// n_compo components
+// n_order succesive time derivatives
+// n_sample time values
+//
+function poly_display_traj(time, traj)
+
+  [n_compo, n_order, n_sample] = size(traj); 
+ 
+  for compo=1:n_compo
+    for order=1:n_order
+      subplot(n_order, n_compo, compo+(order-1)*n_compo);
+      plot2d(time, matrix(traj(compo,order,:), n_sample, 1));
+      xtitle(sprintf('$X^{%d}_{%d}$', order-1, compo));
+    end
+  end
+  
+endfunction
+
+
+//
+// compute the values of a set of polynomials along a time vector
+// 
+//
+function [traj] = poly_gen_traj(time, coefs)
+ 
+  [n_comp, n_order, n_coef] = size(coefs);
+ 
+  traj = zeros(n_comp, n_coef/2, length(time));
+  for compo=1:n_comp 
+    for order=1:n_order
+      for i=1:length(time)
+	traj(compo, order, i) = ...
+	    poly_compute_val(matrix(coefs(compo,order, :),1,n_coef), time(1), time(i));
+      end
+    end
+  end
+
+endfunction
+
+
+//
+// compute v = a_{n}*(t-t_0)^{n} + a_{n-1}*(t-t_0)^{n-1} + ... + a_{0}
+//
+//
+function [v] = poly_compute_val(coefs, t0, t)
+  dt = t-t0;
+  v = coefs($);
+  for i=1:length(coefs)-1
+    v = v * dt;
+    v = v + coefs(length(coefs)-i); // assume coef(1) = a_0
+  end
+  
+endfunction
+
+
+//
+//  compute coefficients for a set of polynomials
+//  having bond values b0 and b1
+//
+//
+function [coefs] = poly_get_coef_from_bound(time, b0,b1)
+
+  [n_comp, n_order] = size(b0);
+  n_coef = 2*n_order
+  coefs = zeros(n_comp, n_order, n_coef); 
+  
+  // refer to paper for notations
+  
+  for compo=1:n_comp
+
+    // invert of the top left corner block  
+    invA1 = zeros(n_order, n_order);
+    for i=1:n_order
+      invA1(i,i) = 1/Arr(i-1,i-1);
+    end
+    // first half of the coefficients
+    coefs(compo, 1, 1:n_order) = (invA1*b0(compo,:)')';
+    
+    // bottom left block : triangular
+    A3 = zeros(n_order, n_order);
+    dt = time($) - time(1);
+    for i=1:n_order
+      for j=i:n_order
+	A3(i,j) = Arr(i-1,j-1) * dt^(j-i);
+      end
+    end
+    // bottom right block 
+    A4 = zeros(n_order, n_order);
+    for i=1:n_order
+      for j=1:n_order
+	A4(i,j) = Arr(i-1,j-1+n_order) * dt^(j+n_order-i);
+      end
+    end
+    coefs(compo, 1, n_order+1:2*n_order) = ...
+	(inv(A4)*(b1(compo,:)' - A3*matrix(coefs(compo,1, 1:n_order), n_order, 1)))';
+    // fill in the coefficients for the succesive time derivatives  
+//    pause
+    
+    for order=2:n_order
+      for pow=0:2*n_order-order
+	coefs(compo, order, pow+1) = Arr(order-1,pow-1+order)*coefs(compo, 1, pow+order);
+      end
+    end
+  end
+  
+endfunction
+
+
+//
+// Arrangement
+//
+//
+function [akn] = Arr(k,n)
+  akn = factorial(n)/factorial(n-k);
+endfunction

sw/simulator/scilab/q3d/test_polynomial.sce

@@ -1,20 +1,20 @@
 clear()
 
 // a0 + a1(t-t0) + ... + an(t-tO)^n
-coefs = 
 
 
 
 
-if 0
+
+
 exec('q3d_utils.sci');
 exec('q3d_polynomials.sci');
 exec('q3d_diff_flatness.sci');
 exec('q3d_fdm.sci');
 exec('q3d_display.sci');
 
-b0 = [0 1 0];
-b1 = [2 0 0];
+b0 = [0 0 0 0 0];
+b1 = [2 0 0 0 0];
 t0 = 0;
 t1 = 2;
 dt = 0.01;
@@ -30,4 +30,4 @@ clf();
 f=get("current_figure");
 f.figure_name="Flat Outputs Trajectory";
 poly_display_traj(time, fo_traj);
-end
+

sw/simulator/scilab/q3d/test_stop_stop.sce

@@ -5,10 +5,10 @@ exec('q3d_diff_flatness.sci');
 exec('q3d_fdm.sci');
 exec('q3d_display.sci');
 
-b0 = [0  0 0 0 0; 0 0 0 0 0];
-b1 = [20 0 0 0 0; 0 0 0 0 0];
+b0 = [0 0 0 0 0; 0 0 0 0 0];
+b1 = [5 0 0 0 0; 0 0 0 0 0];
 t0 = 0;
-t1 = 10;
+t1 = 2.;
 dt = 0.01;
 time = t0:dt:t1;