diff --git a/sw/simulator/scilab/q3d/povray/q3d.inc b/sw/simulator/scilab/q3d/povray/q3d.inc index d9c3e57af8..7eb9409e39 100644 --- a/sw/simulator/scilab/q3d/povray/q3d.inc +++ b/sw/simulator/scilab/q3d/povray/q3d.inc @@ -43,8 +43,8 @@ union { #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 @@ camera 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{ White*lgt1_intense} @@ -75,4 +75,19 @@ light_source{ White*lgt2_intense} light_source{ White*lgt3_intense} light_source{ 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 diff --git a/sw/simulator/scilab/q3d/q3d_diff_flatness.sci b/sw/simulator/scilab/q3d/q3d_diff_flatness.sci index 262f63e172..16b26007c9 100644 --- a/sw/simulator/scilab/q3d/q3d_diff_flatness.sci +++ b/sw/simulator/scilab/q3d/q3d_diff_flatness.sci @@ -51,37 +51,6 @@ function [inp] = df_input_of_fo(fo) 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 - - diff --git a/sw/simulator/scilab/q3d/q3d_display.sci b/sw/simulator/scilab/q3d/q3d_display.sci index 542798383a..4575a59785 100644 --- a/sw/simulator/scilab/q3d/q3d_display.sci +++ b/sw/simulator/scilab/q3d/q3d_display.sci @@ -104,27 +104,27 @@ endfunction 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 diff --git a/sw/simulator/scilab/q3d/q3d_fo_traj_misc.sci b/sw/simulator/scilab/q3d/q3d_fo_traj_misc.sci new file mode 100644 index 0000000000..e5119bf593 --- /dev/null +++ b/sw/simulator/scilab/q3d/q3d_fo_traj_misc.sci @@ -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 diff --git a/sw/simulator/scilab/q3d/q3d_sbb.sci b/sw/simulator/scilab/q3d/q3d_sbb.sci index 9d5efb8a35..d591170cc9 100644 --- a/sw/simulator/scilab/q3d/q3d_sbb.sci +++ b/sw/simulator/scilab/q3d/q3d_sbb.sci @@ -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 diff --git a/sw/simulator/scilab/q3d/test_circle.sce b/sw/simulator/scilab/q3d/test_circle.sce new file mode 100644 index 0000000000..e6edde35bf --- /dev/null +++ b/sw/simulator/scilab/q3d/test_circle.sce @@ -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); diff --git a/sw/simulator/scilab/q3d/test_stop_stop.sce b/sw/simulator/scilab/q3d/test_stop_stop.sce index e6de63272c..70a628eb0f 100644 --- a/sw/simulator/scilab/q3d/test_stop_stop.sce +++ b/sw/simulator/scilab/q3d/test_stop_stop.sce @@ -3,6 +3,7 @@ exec('q3d_utils.sci'); 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 @@ exec('q3d_povray.sci'); t0 = 0; -t1 = 4.5; -dt = 0.01; +t1 = 10.; +dt = 1/512; time = t0:dt:t1; if (0) @@ -21,13 +22,22 @@ if (0) 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 -// 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,$)); - end - +if (0) +// differential equation + 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 @@ for i=2:length(time) 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 @@ set("current_figure",3); 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);