sw/simulator/scilab/q6d/povray/q3d.pov

@@ -1,2 +0,0 @@
-#include "povray/q3d.inc"
-object { Q3D()  rotate <-1.881324,-27.550064,-18.630888> translate <2787.790787,-1108.252014,-4762.512255>}

sw/simulator/scilab/q6d/povray/q6d.inc

@@ -1,4 +1,3 @@
-
 //POVRay include files
 #include "arrays.inc"
 #include "chars.inc"
@@ -21,20 +20,79 @@
 #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 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}}}
+#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;
@@ -43,7 +101,7 @@ union {
 #declare cam_look_y = 0;
 #declare cam_look_z = 0;
 
-#declare cam_a = 32;
+#declare cam_a = 36;
 //#declare cam_a = 10;
 
 camera

sw/simulator/scilab/q6d/q6d_diff_flatness.sci

@@ -79,10 +79,9 @@ function [state] = df_state_of_fo(fo)
   
   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 @@ function [inp] = df_input_of_fo(fo)
   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 @@ function [inp] = df_input_of_fo(fo)
   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);

sw/simulator/scilab/q6d/q6d_fo_traj_misc.sci

@@ -1,3 +1,21 @@
+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)
@@ -7,23 +25,37 @@ function [fo_traj] = fo_traj_circle(time)
   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
     

sw/simulator/scilab/q6d/q6d_povray.sci

@@ -1,6 +1,7 @@
 
+
 function povray_draw( time, diff_flat_ref )
-  
+
   dt_display = 1/25;
 
   t_idx = 1;
@@ -8,18 +9,19 @@ function povray_draw( time, diff_flat_ref )
   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 @@ function povray_draw( time, diff_flat_ref )
     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

sw/simulator/scilab/q6d/test_stop_stop.sce

@@ -17,13 +17,12 @@ 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(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 @@ set("current_figure",1);
 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)