[paparazzi-commits] [4595]

[antoine drouin]

Revision: 4595
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4595
Author:   poine
Date:     2010-02-25 10:35:26 +0000 (Thu, 25 Feb 2010)
Log Message:
-----------


Modified Paths:
--------------
    paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_diff_flatness.sci
    paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_fo_traj_misc.sci
    paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_povray.sci
    paparazzi3/trunk/sw/simulator/scilab/q6d/test_stop_stop.sce

Added Paths:
-----------
    paparazzi3/trunk/sw/simulator/scilab/q6d/povray/q6d.inc

Removed Paths:
-------------
    paparazzi3/trunk/sw/simulator/scilab/q6d/povray/q3d.inc
    paparazzi3/trunk/sw/simulator/scilab/q6d/povray/q3d.pov

Deleted: paparazzi3/trunk/sw/simulator/scilab/q6d/povray/q3d.inc
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q6d/povray/q3d.inc     2010-02-25 
10:24:22 UTC (rev 4594)
+++ paparazzi3/trunk/sw/simulator/scilab/q6d/povray/q3d.inc     2010-02-25 
10:35:26 UTC (rev 4595)
@@ -1,93 +0,0 @@
-
-//POVRay include files
-#include "arrays.inc"
-#include "chars.inc"
-#include "colors.inc"
-#include "finish.inc"
-#include "glass.inc"
-#include "golds.inc"
-#include "math.inc"
-#include "metals.inc"
-#include "rand.inc"
-#include "shapes.inc"
-#include "shapes2.inc"
-#include "shapesq.inc"
-#include "skies.inc"
-#include "stoneold.inc"
-#include "stones.inc"
-#include "stones1.inc"
-#include "stones2.inc"
-#include "strings.inc"
-#include "textures.inc"
-#include "transforms.inc"
-
-
-#macro Q3D()
-union {
-  box{<-250,-10,-10><250,10,10> texture{pigment{Red}}}
-  cylinder{<-250,10,0><-250,15,0>  125 texture{pigment{Green}}}
-  cylinder{<250,10,0><250,15,0>   125 texture{pigment{Green}}}
-
-  box{<-10,-10,-250><10,10,250> texture{pigment{Red}}}
-  cylinder{<0,10,-250><0,15,-250>  125 texture{pigment{Green}}}
-  cylinder{<0,10, 250><0,15, 250>   125 texture{pigment{Green}}}
-
-}
-#end
-
-#declare cam_x = 0;
-#declare cam_y = 5000;
-#declare cam_z = 20000;
-
-#declare cam_look_x = 0;
-#declare cam_look_y = 0;
-#declare cam_look_z = 0;
-
-#declare cam_a = 32;
-//#declare cam_a = 10;
-
-camera
-{
-       location <cam_x,cam_y,cam_z>
-       look_at <cam_look_x,cam_look_y,cam_look_z>
-       angle cam_a
-}
-
-#local lgt1_pos_x = 3400;
-#local lgt1_pos_y = 5100;
-#local lgt1_pos_z = 2600;
-#local lgt1_intense = 0.763488;
-#local lgt2_pos_x = -3400;
-#local lgt2_pos_y = 5100;
-#local lgt2_pos_z = 2600;
-#local lgt2_intense = 0.763488;
-#local lgt3_pos_x = 3400;
-#local lgt3_pos_y = 5100;
-#local lgt3_pos_z = -1700;
-#local lgt3_intense = 0.763488;
-#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}
-light_source{<lgt2_pos_x,lgt2_pos_y,lgt2_pos_z> White*lgt2_intense}
-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

Deleted: paparazzi3/trunk/sw/simulator/scilab/q6d/povray/q3d.pov
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q6d/povray/q3d.pov     2010-02-25 
10:24:22 UTC (rev 4594)
+++ paparazzi3/trunk/sw/simulator/scilab/q6d/povray/q3d.pov     2010-02-25 
10:35:26 UTC (rev 4595)
@@ -1,2 +0,0 @@
-#include "povray/q3d.inc"
-object { Q3D()  rotate <-1.881324,-27.550064,-18.630888> translate 
<2787.790787,-1108.252014,-4762.512255>}
\ No newline at end of file

Added: paparazzi3/trunk/sw/simulator/scilab/q6d/povray/q6d.inc
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q6d/povray/q6d.inc                     
        (rev 0)
+++ paparazzi3/trunk/sw/simulator/scilab/q6d/povray/q6d.inc     2010-02-25 
10:35:26 UTC (rev 4595)
@@ -0,0 +1,151 @@
+//POVRay include files
+#include "arrays.inc"
+#include "chars.inc"
+#include "colors.inc"
+#include "finish.inc"
+#include "glass.inc"
+#include "golds.inc"
+#include "math.inc"
+#include "metals.inc"
+#include "rand.inc"
+#include "shapes.inc"
+#include "shapes2.inc"
+#include "shapesq.inc"
+#include "skies.inc"
+#include "stoneold.inc"
+#include "stones.inc"
+#include "stones1.inc"
+#include "stones2.inc"
+#include "strings.inc"
+#include "textures.inc"
+#include "transforms.inc"
+
+#macro AXIS_POVRAY(mac_x_ver,mac_y_ver,mac_z_ver,mac_x_rot,mac_y_rot,mac_z_rot)
+object {
+  union {
+    union {
+      cylinder{<0.,0.,0.><2500.,0.,0.> 10}
+      text { ttf "timrom.ttf" "X" 1, 0 scale <2,2,2> translate(<20, 0, 0>)}
+      texture{ pigment{rgb<1,0,0>}}
+    }   
+    union {
+      cylinder{<0.,0,0.><0.,2500.,0.> 10}
+      text { ttf "timrom.ttf" "Y" 1, 0 scale <2,2,2> translate(<0, 20, 0>)}
+      texture{ pigment{rgb<0,1,0>}}
+    }   
+    union {
+      cylinder{<0.,0,0.><0.,0.,2500.> 10}
+      text { ttf "timrom.ttf" "Z" 1, 0 scale <2,2,2> translate(<0, 0, 20>)}
+      texture{ pigment{rgb<0,0,1>}}
+    }   
+  translate(<mac_x_ver,mac_y_ver,mac_z_ver>)
+  rotate(<mac_x_rot,mac_y_rot,mac_z_rot>)      
+  }
+} 
+#end
+
+
+#macro AXIS_NED()
+object {
+  union {
+    union {
+      cylinder{<0.,0.,0.><2500.,0.,0.> 10}
+      texture{ pigment{rgb<1,0,0>}}
+    }   
+    union {
+      cylinder{<0.,0,0.><0.,2500.,0.> 10}
+      texture{ pigment{rgb<0,1,0>}}
+    }   
+    union {
+      cylinder{<0.,0,0.><0.,0.,2500.> 10}
+      texture{ pigment{rgb<0,0,1>}}
+    }   
+  }
+  matrix < 1, 0, 0,
+           0, 0,-1,
+           0,-1, 0,
+           0, 0, 0 >
+} 
+#end
+
+
+
+#macro Q6D(xned, yned, zned, phi, theta, psi)
+
+object {
+  union {
+    box{<-250,-10,-10><0,10,10> texture{pigment{Blue}}}
+    box{<0,-10,-10><250,10,10> texture{pigment{Red}}}
+    cylinder{<-250,0,-10><-250,0,-15>  125 texture{pigment{Green}}}
+    cylinder{<250, 0,-10><250,0,-15>   125 texture{pigment{Green}}}
+
+    box{<-10,-250,-10><10,250,10> texture{pigment{Blue}}}
+    cylinder{<0,-250,-10><0,-250,-15>  125 texture{pigment{Green}}}
+    cylinder{<0, 250,-10><0, 250,-15>  125 texture{pigment{Green}}}
+  }
+  rotate <phi,theta, psi>
+  translate <xned,yned,zned>
+  matrix < 1, 0, 0,
+           0, 0,-1,
+           0,-1, 0,
+           0, 0, 0 >
+}
+#end
+
+
+#declare cam_x = 0;
+#declare cam_y = 5000;
+#declare cam_z = 20000;
+
+#declare cam_look_x = 0;
+#declare cam_look_y = 0;
+#declare cam_look_z = 0;
+
+#declare cam_a = 36;
+//#declare cam_a = 10;
+
+camera
+{
+       location <cam_x,cam_y,cam_z>
+       look_at <cam_look_x,cam_look_y,cam_look_z>
+       angle cam_a
+}
+
+#local lgt1_pos_x = 3400;
+#local lgt1_pos_y = 5100;
+#local lgt1_pos_z = 2600;
+#local lgt1_intense = 0.763488;
+#local lgt2_pos_x = -3400;
+#local lgt2_pos_y = 5100;
+#local lgt2_pos_z = 2600;
+#local lgt2_intense = 0.763488;
+#local lgt3_pos_x = 3400;
+#local lgt3_pos_y = 5100;
+#local lgt3_pos_z = -1700;
+#local lgt3_intense = 0.763488;
+#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}
+light_source{<lgt2_pos_x,lgt2_pos_y,lgt2_pos_z> White*lgt2_intense}
+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/q6d/q6d_diff_flatness.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_diff_flatness.sci      
2010-02-25 10:24:22 UTC (rev 4594)
+++ paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_diff_flatness.sci      
2010-02-25 10:35:26 UTC (rev 4595)
@@ -79,10 +79,9 @@
   
   phid   = sign(z2dmg)*(adypsi*av-adv*aypsi)/(aypsi^2+av^2);
   thetad = (adxpsi*z2dmg-z3d*axpsi)/(axpsi^2+z2dmg^2);
-  
+
   cphi = cos(state(DF_REF_PHI));
   sphi = sin(state(DF_REF_PHI));
-
   ctheta = cos(state(DF_REF_THETA));
   stheta = sin(state(DF_REF_THETA));
   
@@ -139,7 +138,6 @@
   adv = (axpsi*adxpsi + z2dmg*z3d)/av;
   z4d = fo(3,5);
   a = (axpsi*a2dxpsi + adxpsi^2 + (z2dmg)*z4d +z3d^2)*av;
-  //a = (axpsi*a2dxpsi + adxpsi^2 + (z2dmg)*z4d +z3d)*av;
   b = -adv*(axpsi*adxpsi + z2dmg*z3d);
   a2dv = (a+b)/av^2;
   
@@ -147,7 +145,6 @@
   theta = atan(axpsi/z2dmg);
   
   phid   = sign(z2dmg)*(adypsi*av-adv*aypsi)/(aypsi^2+av^2);
-  //thetad = (adxpsi*z2dmg-z3d*aypsi)/(axpsi^2+z2dmg^2);
   thetad = (adxpsi*z2dmg-z3d*axpsi)/(axpsi^2+z2dmg^2);
     
   a = (a2dypsi*av-a2dv*aypsi)*(aypsi^2+av^2);

Modified: paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_fo_traj_misc.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_fo_traj_misc.sci       
2010-02-25 10:24:22 UTC (rev 4594)
+++ paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_fo_traj_misc.sci       
2010-02-25 10:35:26 UTC (rev 4595)
@@ -1,5 +1,23 @@
+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
 
+
 function [fo_traj] = fo_traj_circle(time)
 
   fo_traj = zeros(DF_FO_SIZE, DF_FO_ORDER, length(time));
@@ -7,23 +25,37 @@
   radius = 3;
   omega = rad_of_deg(60);
   c = [0 0]';
+  omega_z = rad_of_deg(105); 
+  dz = 1.;
   
   for i=1:length(time)
 
     fo_traj(DF_FO_X,1,i) = c(1) + radius*cos(omega*time(i));
     fo_traj(DF_FO_Y,1,i) = c(2) + radius*sin(omega*time(i));
+    fo_traj(DF_FO_Z,1,i) = dz*sin(omega_z*time(i));
+//    fo_traj(DF_FO_PSI,1,i) = omega*time(i);
+    
 
     fo_traj(DF_FO_X,2,i) = c(1) - omega*radius*sin(omega*time(i));
     fo_traj(DF_FO_Y,2,i) = c(2) + omega*radius*cos(omega*time(i));
-  
+    fo_traj(DF_FO_Z,2,i) = omega_z*dz*cos(omega_z*time(i));
+//    fo_traj(DF_FO_PSI,2,i) = omega;
+
+
     fo_traj(DF_FO_X,3,i) = c(1) - omega^2*radius*cos(omega*time(i));
     fo_traj(DF_FO_Y,3,i) = c(2) - omega^2*radius*sin(omega*time(i));
-    
+    fo_traj(DF_FO_Z,3,i) = -omega_z^2*dz*sin(omega_z*time(i));
+//    fo_traj(DF_FO_PSI,3,i) = 0;
+
+
     fo_traj(DF_FO_X,4,i) = c(1) + omega^3*radius*sin(omega*time(i));
     fo_traj(DF_FO_Y,4,i) = c(2) - omega^3*radius*cos(omega*time(i));
+    fo_traj(DF_FO_Z,4,i) = -omega_z^3*dz*cos(omega_z*time(i));
     
+
     fo_traj(DF_FO_X,5,i) = c(1) + omega^4*radius*cos(omega*time(i));
     fo_traj(DF_FO_Y,5,i) = c(2) + omega^4*radius*sin(omega*time(i));
+    fo_traj(DF_FO_Z,5,i) = omega_z^4*dz*sin(omega_z*time(i));
   
   end
     

Modified: paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_povray.sci
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_povray.sci     2010-02-25 
10:24:22 UTC (rev 4594)
+++ paparazzi3/trunk/sw/simulator/scilab/q6d/q6d_povray.sci     2010-02-25 
10:35:26 UTC (rev 4595)
@@ -1,6 +1,7 @@
 
+
 function povray_draw( time, diff_flat_ref )
-  
+
   dt_display = 1/25;
 
   t_idx = 1;
@@ -8,18 +9,19 @@
   t = time(t_idx);
   while (t_idx<length(time))
     printf('drawing %d (%f)\n', f_idx, time(t_idx));
-    x = -1000*diff_flat_ref(DF_REF_X,t_idx);
-    y = 1000*diff_flat_ref(DF_REF_Y,t_idx);
-    z = -1000*diff_flat_ref(DF_REF_Z,t_idx);
-    phi =  deg_of_rad(-diff_flat_ref(DF_REF_PHI,t_idx));
-    theta =  deg_of_rad(-diff_flat_ref(DF_REF_THETA,t_idx));
-    psi =  deg_of_rad(-diff_flat_ref(DF_REF_PSI,t_idx));
-//    printf('( %f %f %f)\n', x, y, alpha);
-    fid = mopen('povray/q3d.pov', "w");
-    mfprintf(fid, "#include ""povray/q3d.inc""\n");
-    mfprintf(fid, "object { Q3D()  rotate <%f,%f,%f> translate 
<%f,%f,%f>}",-phi,psi,-theta,x,y,z);
+    px =  1000*diff_flat_ref(DF_REF_X,t_idx);
+    py =  1000*diff_flat_ref(DF_REF_Y,t_idx);
+    pz =  1000*diff_flat_ref(DF_REF_Z,t_idx);
+    _phi   = deg_of_rad(diff_flat_ref(DF_REF_PHI,t_idx));
+    _theta = deg_of_rad(diff_flat_ref(DF_REF_THETA,t_idx));
+    _psi   = deg_of_rad(diff_flat_ref(DF_REF_PSI,t_idx));
+//    printf('( %f %f %f - %f %f %f)\n', px, py, pz, alpha, beta, gamma);
+    fid = mopen('povray/q6d.pov', "w");
+    mfprintf(fid, "#include ""povray/q6d.inc""\n");
+    mfprintf(fid, "object { 
Q6D(%f,%f,%f,%f,%f,%f)}\n",px,py,pz,_phi,_theta,_psi);
+    mfprintf(fid, "object {  AXIS_NED()}\n");
     mclose(fid);
-    cmd = sprintf('povray povray/q3d.pov +Opovray/foo%04d.png Display=false 
+W800 +H600 +Q9 +A0.3 +R5', f_idx);
+    cmd = sprintf('povray povray/q6d.pov +Opovray/img%04d.png Display=false 
+W800 +H600 +Q9 +A0.3 +R5', f_idx);
     a = unix_g(cmd);
     while (t_idx<length(time) & time(t_idx) < f_idx*dt_display)
       t_idx = t_idx + 1;
@@ -27,7 +29,7 @@
     f_idx = f_idx + 1;
   end
 
-  mplayer_cmd = "mencoder ""mf://povray/foo*.png"" -mf fps=25 -o 
povray/test.avi -ovc lavc -lavcopts vcodec=msmpeg4v2:vbitrate=800";
+  mplayer_cmd = "mencoder ""mf://povray/img*.png"" -mf fps=25 -o 
povray/q6d.avi -ovc lavc -lavcopts vcodec=msmpeg4v2:vbitrate=800";
   unix_g(mplayer_cmd);
   
 endfunction

Modified: paparazzi3/trunk/sw/simulator/scilab/q6d/test_stop_stop.sce
===================================================================
--- paparazzi3/trunk/sw/simulator/scilab/q6d/test_stop_stop.sce 2010-02-25 
10:24:22 UTC (rev 4594)
+++ paparazzi3/trunk/sw/simulator/scilab/q6d/test_stop_stop.sce 2010-02-25 
10:35:26 UTC (rev 4595)
@@ -17,13 +17,12 @@
 max_speed = [5 2.5];
 max_accel = [ 9.81*tan(rad_of_deg(30)) 0.5*9.81];
 
-//b0 = [ 0    0   0];
-//b1 = [-10   1  -2];
-b0 = [ 0   0   0 ];
-b1 = [ 0   0  5 ];
-[fo_traj] = sbb_gen_traj(time, dyn, max_speed, max_accel, b0, b1);
-[traj] = fo_traj_circle(time);
-fo_traj(1:2,:,:) = traj(1:2,:,:);
+b0 = [ -5    0   0];
+b1 = [  5    0   0];
+//b0 = [ 0   -5   0 ];
+//b1 = [ 0    5  0 ];
+//[fo_traj] = sbb_gen_traj(time, dyn, max_speed, max_accel, b0, b1);
+[fo_traj] = fo_traj_circle(time);
 
 set("current_figure",0);
 clf();
@@ -41,6 +40,8 @@
 clf();
 display_df_ref(time, diff_flat_ref);
 
+povray_draw( time, diff_flat_ref );
+
 set("current_figure",2);
 clf();
 display_df_cmd(time, diff_flat_cmd)