diff --git a/sw/simulator/scilab/q3d/q3d_diff_flatness.sci b/sw/simulator/scilab/q3d/q3d_diff_flatness.sci new file mode 100644 index 0000000000..262f63e172 --- /dev/null +++ b/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 + + + + + diff --git a/sw/simulator/scilab/q3d/q3d_polynomials.sci b/sw/simulator/scilab/q3d/q3d_polynomials.sci new file mode 100644 index 0000000000..280ba6c815 --- /dev/null +++ b/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 diff --git a/sw/simulator/scilab/q3d/test_polynomial.sce b/sw/simulator/scilab/q3d/test_polynomial.sce index 82a2f4fd14..040b2762af 100644 --- a/sw/simulator/scilab/q3d/test_polynomial.sce +++ b/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 + diff --git a/sw/simulator/scilab/q3d/test_stop_stop.sce b/sw/simulator/scilab/q3d/test_stop_stop.sce index 2db2845482..c48f3fe55f 100644 --- a/sw/simulator/scilab/q3d/test_stop_stop.sce +++ b/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;