60 lines
2.2 KiB
Matlab
60 lines
2.2 KiB
Matlab
function writeTrajectoryCoefficientsIntoFile(a, b, c_pol)
|
|
% -----------------------------------------------------------------------
|
|
% the function takes the parameters of the trajectory for identification
|
|
% that consists of trancated Fourier series and 5th order polynomial
|
|
% and writes in into three files that are used by UR in order to
|
|
% perform trajectory.
|
|
% Input:
|
|
% a - sine coefficients in the truncated fourier series
|
|
% b - cosine coefficients in the truncated fourier series
|
|
% c_pol - fifth order polynomial coefficinets
|
|
% -----------------------------------------------------------------------
|
|
prcsn = 10;
|
|
a_coefs = {};
|
|
b_coefs = {};
|
|
a_coefs{1} = 'a1 = ['; a_coefs{2} = 'a2 = ['; a_coefs{3} = 'a3 = [';
|
|
a_coefs{4} = 'a4 = ['; a_coefs{5} = 'a5 = ['; a_coefs{6} = 'a6 = [';
|
|
b_coefs{1} = 'b1 = ['; b_coefs{2} = 'b2 = ['; b_coefs{3} = 'b3 = [';
|
|
b_coefs{4} = 'b4 = ['; b_coefs{5} = 'b5 = ['; b_coefs{6} = 'b6 = [';
|
|
for i = 1:size(a,2)
|
|
if i < size(a,2)
|
|
for j = 1:6
|
|
a_coefs{j} = strcat(a_coefs{j}, num2str(a(j,i), prcsn), ',');
|
|
b_coefs{j} = strcat(b_coefs{j}, num2str(b(j,i), prcsn), ',');
|
|
end
|
|
elseif i == size(a,2)
|
|
for j = 1:6
|
|
a_coefs{j} = strcat(a_coefs{j}, num2str(a(j,i), prcsn), ']\n');
|
|
b_coefs{j} = strcat(b_coefs{j}, num2str(b(j,i), prcsn), ']\n');
|
|
end
|
|
end
|
|
end
|
|
|
|
c_coefs = {};
|
|
c_coefs{1} = 'c1 = ['; c_coefs{2} = 'c2 = ['; c_coefs{3} = 'c3 = [';
|
|
c_coefs{4} = 'c4 = ['; c_coefs{5} = 'c5 = ['; c_coefs{6} = 'c6 = [';
|
|
for i = 1:size(c_pol,2)
|
|
if i < size(c_pol,2)
|
|
for j = 1:6
|
|
c_coefs{j} = strcat(c_coefs{j}, num2str(c_pol(j,i), prcsn+2), ',');
|
|
end
|
|
elseif i == size(c_pol,2)
|
|
for j = 1:6
|
|
c_coefs{j} = strcat(c_coefs{j}, num2str(c_pol(j,i), prcsn+2), ']\n');
|
|
end
|
|
end
|
|
end
|
|
|
|
fileID_a = fopen('trajectory_optmzn/coeffs4_UR/a_coeffs.script','w');
|
|
fileID_b = fopen('trajectory_optmzn/coeffs4_UR/b_coeffs.script','w');
|
|
fileID_c = fopen('trajectory_optmzn/coeffs4_UR/c_coeffs.script','w');
|
|
for i = 1:6
|
|
fprintf(fileID_a, a_coefs{i});
|
|
fprintf(fileID_b, b_coefs{i});
|
|
fprintf(fileID_c, c_coefs{i});
|
|
end
|
|
fclose(fileID_a);
|
|
fclose(fileID_b);
|
|
fclose(fileID_c);
|
|
|