[paparazzi-commits] [4564]

[antoine drouin]

Revision: 4564
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4564
Author:   poine
Date:     2010-02-23 17:46:24 +0000 (Tue, 23 Feb 2010)
Log Message:
-----------


Modified Paths:
--------------
    paparazzi3/trunk/sw/simulator/scilab/q3d/povray/q3d.inc
    paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_diff_flatness.sci
    paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_display.sci
    paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_sbb.sci
    paparazzi3/trunk/sw/simulator/scilab/q3d/test_stop_stop.sce

Added Paths:
-----------
    paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_fo_traj_misc.sci
    paparazzi3/trunk/sw/simulator/scilab/q3d/test_circle.sce

Modified: paparazzi3/trunk/sw/simulator/scilab/q3d/povray/q3d.inc
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q3d/povray/q3d.inc     2010-02-23 
17:44:54 UTC (rev 4563)
+++ paparazzi3/trunk/sw/simulator/scilab/q3d/povray/q3d.inc     2010-02-23 
17:46:24 UTC (rev 4564)
@@ -43,8 +43,8 @@
 #declare cam_look_y = 0;
 #declare cam_look_z = 0;
 
-//#declare cam_a = 32;
-#declare cam_a = 10;
+#declare cam_a = 32;
+//#declare cam_a = 10;
 
 camera
 {
@@ -53,21 +53,21 @@
        angle cam_a
 }
 
-#local lgt1_pos_x = 340;
-#local lgt1_pos_y = 510;
-#local lgt1_pos_z = 260;
+#local lgt1_pos_x = 3400;
+#local lgt1_pos_y = 5100;
+#local lgt1_pos_z = 2600;
 #local lgt1_intense = 0.763488;
-#local lgt2_pos_x = -340;
-#local lgt2_pos_y = 510;
-#local lgt2_pos_z = 260;
+#local lgt2_pos_x = -3400;
+#local lgt2_pos_y = 5100;
+#local lgt2_pos_z = 2600;
 #local lgt2_intense = 0.763488;
-#local lgt3_pos_x = 340;
-#local lgt3_pos_y = 510;
-#local lgt3_pos_z = -170;
+#local lgt3_pos_x = 3400;
+#local lgt3_pos_y = 5100;
+#local lgt3_pos_z = -1700;
 #local lgt3_intense = 0.763488;
-#local lgt4_pos_x = -340;
-#local lgt4_pos_y = 510;
-#local lgt4_pos_z = -170;
+#local lgt4_pos_x = -3400;
+#local lgt4_pos_y = 5100;
+#local lgt4_pos_z = -1700;
 #local lgt4_intense = 0.763488;
 
 light_source{<lgt1_pos_x,lgt1_pos_y,lgt1_pos_z> White*lgt1_intense}
@@ -75,4 +75,19 @@
 light_source{<lgt3_pos_x,lgt3_pos_y,lgt3_pos_z> White*lgt3_intense}
 light_source{<lgt4_pos_x,lgt4_pos_y,lgt4_pos_z> White*lgt4_intense}
 
+
+//plane { -y, 3000 texture{T_Chrome_2D normal{waves 0.1 frequency 3000.0 scale 
30.0}} translate<0,0,0>}
+//plane { -z, 3000 texture{T_Chrome_2D normal{waves 0.1 frequency 3000.0 scale 
30.0}} translate<0,0,0>}
+
+plane { -y, 3000 texture{T_Chrome_2D}}
+//plane { -z, 3000 texture{T_Chrome_2D}}
+
+sky_sphere {pigment {Navy}
+pigment {bozo turbulence 0.65 octaves 7 omega 0.7 lambda 2
+color_map {
+[0.0 0.1 color rgb <0.85, 0.85, 0.85> color rgb <0.75, 0.75, 0.75>]
+[0.1 0.5 color rgb <0.75, 0.75, 0.75> color rgbt <1, 1, 1, 1>]
+[0.5 1.0 color rgbt <1, 1, 1, 1> color rgbt <1, 1, 1, 1>]}
+scale <0.1, 0.5, 0.1>} rotate 90*z}
+
 background{Gray50}
\ No newline at end of file

Modified: paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_diff_flatness.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_diff_flatness.sci      
2010-02-23 17:44:54 UTC (rev 4563)
+++ paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_diff_flatness.sci      
2010-02-23 17:46:24 UTC (rev 4564)
@@ -51,37 +51,6 @@
 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
 
-
-
-
-

Modified: paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_display.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_display.sci    2010-02-23 
17:44:54 UTC (rev 4563)
+++ paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_display.sci    2010-02-23 
17:46:24 UTC (rev 4564)
@@ -104,27 +104,27 @@
 function display_fo_ref(time, diff_flat_ref)
 
   subplot(2,3,1);
-  plot2d(fdm_time, diff_flat_ref(FDM_SX, :));
+  plot2d(time, diff_flat_ref(FDM_SX, :));
   xtitle('X');
 
   subplot(2,3,2);
-  plot2d(fdm_time, diff_flat_ref(FDM_SZ, :));
+  plot2d(time, diff_flat_ref(FDM_SZ, :));
   xtitle('Z');
 
   subplot(2,3,3);
-  plot2d(fdm_time, deg_of_rad(diff_flat_ref(FDM_STHETA, :)));
+  plot2d(time, deg_of_rad(diff_flat_ref(FDM_STHETA, :)));
   xtitle('Theta');
 
   subplot(2,3,4);
-  plot2d(fdm_time, diff_flat_ref(FDM_SXD, :));
+  plot2d(time, diff_flat_ref(FDM_SXD, :));
   xtitle('Xd');
 
   subplot(2,3,5);
-  plot2d(fdm_time, diff_flat_ref(FDM_SZD, :));
+  plot2d(time, diff_flat_ref(FDM_SZD, :));
   xtitle('Zd');
 
   subplot(2,3,6);
-  plot2d(fdm_time, deg_of_rad(diff_flat_ref(FDM_STHETAD, :)));
+  plot2d(time, deg_of_rad(diff_flat_ref(FDM_STHETAD, :)));
   xtitle('Thetad');
   
   endfunction
\ No newline at end of file

Added: paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_fo_traj_misc.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_fo_traj_misc.sci               
                (rev 0)
+++ paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_fo_traj_misc.sci       
2010-02-23 17:46:24 UTC (rev 4564)
@@ -0,0 +1,50 @@
+
+
+function [fo_traj] = fo_traj_circle(time, _center, radius, omega)
+  
+  n_comp = 2;
+  order = 5;
+  fo_traj = zeros(n_comp, order, length(time));
+
+  for i=1:length(time)
+    
+    alpha = omega*time(i);
+    fo_traj(1,1,i) = _center(1) + radius * cos(alpha);
+    fo_traj(2,1,i) = _center(1) + radius * sin(alpha);
+
+    fo_traj(1,2,i) = -omega * radius * sin(alpha);
+    fo_traj(2,2,i) =  omega * radius * cos(alpha);
+    
+    fo_traj(1,3,i) = -omega^2 * radius * cos(alpha);
+    fo_traj(2,3,i) = -omega^2 * radius * sin(alpha);
+
+    fo_traj(1,4,i) =  omega^3 * radius * sin(alpha);
+    fo_traj(2,4,i) = -omega^3 * radius * cos(alpha);
+
+    fo_traj(1,5,i) =  omega^4 * radius * cos(alpha);
+    fo_traj(2,5,i) =  omega^4 * radius * sin(alpha);
+  
+  end  
+
+
+endfunction
+
+
+function [time_out, traj_out] = merge_traj(time_in, traj_in)
+  time_out = [];
+  for t=time_in
+    time_out = [time_out t];
+  end
+  traj_out = 0;
+  [nb_comp, nb_order, foo] = size(traj_in(1));
+  traj_out = zeros(nb_comp, nb_order, length(time_out));
+
+  l=1;
+  for i=1:length(time_in)
+    for j=1:length(time_in(i))
+      ti = traj_in(i);
+      traj_out(:,:,l) = ti(:,:,j);
+      l=l+1;
+    end
+  end
+endfunction

Modified: paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_sbb.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_sbb.sci        2010-02-23 
17:44:54 UTC (rev 4563)
+++ paparazzi3/trunk/sw/simulator/scilab/q3d/q3d_sbb.sci        2010-02-23 
17:46:24 UTC (rev 4564)
@@ -4,59 +4,61 @@
 //
 //
 
-sbb_omega = rad_of_deg(500);
-sbb_ksi = 0.70;
-sbb_tolerance = 0.05;
+sbb_tolerance = 0.025;
 
-function [fo_traj] = sbb_gen_traj(time, max_xd, max_theta, b0, b1)
+function [fo_traj] = sbb_gen_traj(time, dyn, max_speed, max_accel, b0, b1)
   n_comp = 2;
   order = 5;
   fo_traj = zeros(n_comp, order, length(time));
 
-  // compute trajectory caracteristics
-  dx = b1(1)-b0(1);
-  step_dt = -log(sbb_tolerance)/(sbb_ksi*sbb_omega*sqrt(1-sbb_ksi^2));
-  step_xdd = dx / (2*step_dt^2);
-  max_xdd = 9.81 * tan(max_theta);
+  for compo=1:n_comp 
+    // compute trajectory caracteristics
+    dist = b1(compo)-b0(compo);
+    if (abs(dist) > 0.01)
+      step_dt = 
-log(sbb_tolerance)/(dyn(compo,2)*dyn(compo,1)*sqrt(1-dyn(compo,2)^2));
+      step_xdd = dist / (2*step_dt^2);
   
-  if step_xdd < max_xdd
-    if step_xdd > max_xd/step_dt
-      step_xdd = max_xd/step_dt;
-    end
-  else
-    step_xdd = max_xdd;
-    if step_xdd <= max_xd/step_dt
-      step_dt = max_xd/step_xdd;
+      if step_xdd < max_accel(compo)
+        if step_xdd > max_speed(compo)/step_dt
+          step_xdd = max_speed(compo)/step_dt;
+        end
+      else
+        step_xdd = max_accel(compo);
+        if step_xdd <= max_speed(compo)/step_dt
+          step_dt = max_speed(compo)/step_xdd;
+        else
+          step_xdd = max_speed(compo)/step_dt;
+        end
+      end
+      t_tot = (dist - 2*step_dt*(step_xdd*step_dt))/(step_xdd*step_dt) + 
4*step_dt;
     else
-      step_xdd = max_xd/step_dt;
+      step_dt = 0;
+      step_xdd = 0;
+      t_tot = 0;
     end
-  end
-
-  t_tot = (dx - 2*step_dt*(step_xdd*step_dt))/(step_xdd*step_dt) + 4*step_dt;
-
-  printf('dx        :%f\n', dx);
-  printf('step_dt   :%f\n', step_dt);
-  printf('step_xdd  :%f\n', step_xdd);
-  printf('total time:%f\n', t_tot);
+    printf('dist      :%f\n', dist);
+    printf('step_dt   :%f\n', step_dt);
+    printf('step_xdd  :%f\n', step_xdd);
+    printf('total time:%f\n', t_tot);
   
   
-  fo_traj(1,1,1) = b0(1);
-  for i=2:length(time)
-    if time(i) < step_dt
-      sp = step_xdd;      
-    elseif time(i) < t_tot - step_dt & time(i) >= t_tot -  2 * step_dt
-      sp = -step_xdd;      
-    else
-      sp = 0;
+    fo_traj(compo,1,1) = b0(compo);
+      for i=2:length(time)
+        if time(i)-time(1) < step_dt
+          sp = step_xdd;      
+        elseif time(i)-time(1) < t_tot - step_dt & time(i)-time(1) >= t_tot -  
2 * step_dt
+          sp = -step_xdd;      
+        else
+          sp = 0;
+        end
+        fo_traj(compo,:,i) = propagate_traj(fo_traj(compo,:,i-1), 
dyn(compo,:), sp, time(i) - time(i-1)); 
+      end
     end
-    fo_traj(1,:,i) = propagate_traj(fo_traj(1,:,i-1), sp, time(i) - 
time(i-1)); 
-  end
-
+    
 endfunction
 
-
-function [Xi1] = propagate_traj(Xi, sp, dt)
+function [Xi1] = propagate_traj(Xi, dyn, sp, dt)
   Xi1 = zeros(1,5);
   Xi1(1:4) = Xi(1:4) +  Xi(2:5)*dt;
-  Xi1(5) = -2*sbb_ksi*sbb_omega*Xi1(4)-sbb_omega^2*(Xi1(3)-sp);
+  Xi1(5) = -2*dyn(2)*dyn(1)*Xi1(4)-dyn(1)^2*(Xi1(3)-sp);
 endfunction

Added: paparazzi3/trunk/sw/simulator/scilab/q3d/test_circle.sce
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q3d/test_circle.sce                    
        (rev 0)
+++ paparazzi3/trunk/sw/simulator/scilab/q3d/test_circle.sce    2010-02-23 
17:46:24 UTC (rev 4564)
@@ -0,0 +1,79 @@
+clear()
+exec('q3d_utils.sci');
+
+exec('q3d_polynomials.sci');
+exec('q3d_fo_traj_misc.sci');
+
+exec('q3d_diff_flatness.sci');
+exec('q3d_fdm.sci');
+exec('q3d_display.sci');
+exec('q3d_povray.sci');
+
+
+t0 = 0;
+t1 = 5.;
+t2 = 15.;
+t3 = 20.;
+dt = 1/512;
+time1 = t0:dt:t1;
+time2 = t1:dt:t2;
+time3 = t2:dt:t3;
+
+[fo_traj2] = fo_traj_circle(time2, [0 0], 2, rad_of_deg(45));
+
+b0 = [-5 0 0 0 0; 0 0 0 0 0];
+b1 = [ fo_traj2(1,:,1); fo_traj2(2,:,1)];
+[coefs] = poly_get_coef_from_bound(time1, b0, b1);
+[fo_traj1] = poly_gen_traj(time1, coefs);
+
+b0 = [ fo_traj2(1,:,$); fo_traj2(2,:,$)];
+b1 = [-5 0 0 0 0; 0 0 0 0 0];
+[coefs] = poly_get_coef_from_bound(time3, b0, b1);
+[fo_traj3] = poly_gen_traj(time3, coefs);
+
+
+[time, fo_traj] = merge_traj(list(time1, time2, time3), list(fo_traj1, 
fo_traj2, fo_traj3));
+ 
+ 
+diff_flat_cmd = zeros(2,length(time));
+diff_flat_ref = zeros(FDM_SSIZE, length(time));
+for i=1:length(time)
+  diff_flat_cmd(:,i) = df_input_of_fo(fo_traj(:,:,i));
+  diff_flat_ref(:,i) = df_state_of_fo(fo_traj(:,:,i));
+end
+
+fdm_init(time, df_state_of_fo(fo_traj(:,:,1))); 
+for i=2:length(time)
+  u1 = diff_flat_cmd(1,i-1);
+  u2 = diff_flat_cmd(2,i-1);
+  m1 = 0.5*(u1+u2);
+  m2 = 0.5*(u1-u2);
+  fdm_run(i, [m1 m2]')
+end
+
+set("current_figure",0);
+clf();
+f=get("current_figure");
+f.figure_name="Flat Outputs Trajectory";
+display_fo(time, fo_traj);
+
+set("current_figure",1);
+clf();
+f=get("current_figure");
+f.figure_name="Commands";
+display_commands(time, diff_flat_cmd);
+
+set("current_figure",2);
+clf();
+f=get("current_figure");
+f.figure_name="Reference";
+display_fo_ref(time, diff_flat_ref);
+
+set("current_figure",3);
+clf();
+f=get("current_figure");
+f.figure_name="FDM";
+display_fdm();
+
+
+povray_draw(time, diff_flat_ref);

Modified: paparazzi3/trunk/sw/simulator/scilab/q3d/test_stop_stop.sce
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q3d/test_stop_stop.sce 2010-02-23 
17:44:54 UTC (rev 4563)
+++ paparazzi3/trunk/sw/simulator/scilab/q3d/test_stop_stop.sce 2010-02-23 
17:46:24 UTC (rev 4564)
@@ -3,6 +3,7 @@
 
 exec('q3d_polynomials.sci');
 exec('q3d_sbb.sci');
+exec('q3d_fo_traj_misc.sci');
 
 exec('q3d_diff_flatness.sci');
 exec('q3d_fdm.sci');
@@ -11,8 +12,8 @@
 
 
 t0 = 0;
-t1 = 4.5;
-dt = 0.01;
+t1 = 10.;
+dt = 1/512;
 time = t0:dt:t1;
 
 if (0)
@@ -21,13 +22,22 @@
   b1 = [2 0 0 0 0; 0 0 0 0 0];
   [coefs] = poly_get_coef_from_bound(time, b0, b1);
   [fo_traj] = poly_gen_traj(time, coefs);
-else
+end
+if (0)
 // differential equation
-  [fo_traj] = sbb_gen_traj(time, 5, rad_of_deg(29.983325), [-1 0], [1 0]);
-  printf('xfinal:%f\n',fo_traj(1,1,$)); 
-
+  dyn = [rad_of_deg(500) 0.7; rad_of_deg(500) 0.7];
+  max_speed = [5 2.5];
+  max_accel = [ 9.81*tan(rad_of_deg(29.983325)) 0.5*9.81];
+  b0 = [-5 0];
+  b1 = [ 5 -2];
+  [fo_traj] = sbb_gen_traj(time, dyn, max_speed, max_accel, b0, b1);
+  printf('xfinal %f, zfinal:%f\n',fo_traj(1,1,$), fo_traj(2,1,$));
 end
-
+if (1)
+ [fo_traj] = fo_traj_circle(time, [0 0], 2, rad_of_deg(45));
+end
+ 
+ 
 diff_flat_cmd = zeros(2,length(time));
 diff_flat_ref = zeros(FDM_SSIZE, length(time));
 for i=1:length(time)
@@ -44,9 +54,6 @@
   fdm_run(i, [m1 m2]')
 end
 
-
-povray_draw(time, diff_flat_ref);
-
 set("current_figure",0);
 clf();
 f=get("current_figure");
@@ -69,4 +76,7 @@
 clf();
 f=get("current_figure");
 f.figure_name="FDM";
-display_fdm();
\ No newline at end of file
+display_fdm();
+
+
+povray_draw(time, diff_flat_ref);