Add files via upload

Coefficients Identification/Gen_cubic.m

@@ -0,0 +1,167 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%File for generating cubic trajectory%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+close all
+clear all
+
+cd lab_traj
+
+nameDEF = 'q_c_def_72.txt';
+nameVREP = 'q_c_vrep_72.txt';
+nameCPP = 'q_c_72.txt';
+
+
+%Trajectory definition
+deltaT = 0.005;
+N = 7;
+t_step = 3;
+num_traj=7;
+T = t_step*num_traj;
+samples = T/deltaT;
+
+%Joint limits
+q_lim = [170 120 170 120 170 120 170]*pi/180;   %rad
+qd_lim = [110, 110, 128, 128, 204, 184, 184]*pi/180;  %rad/s
+
+%Init trajectories
+TRAJ1 = zeros(N, t_step/deltaT);
+TRAJ2 = zeros(N, t_step/deltaT);
+TRAJ3 = zeros(N, t_step/deltaT);
+TRAJ4 = zeros(N, t_step/deltaT);
+TRAJ5 = zeros(N, t_step/deltaT);
+TRAJ6 = zeros(N, t_step/deltaT);
+TRAJ7 = zeros(N, t_step/deltaT);
+
+
+%Knots
+q_0 = [pi/2; pi/4; 150*pi/180; -45*pi/180; 90*pi/180; -50*pi/180; 34*pi/180];
+q_fin1 = [pi/2; 3/7*pi; pi/2; -100*pi/180; 150*pi/180; 0*pi/180; 150*pi/180];
+q_fin2 = [pi/6; -pi/4; pi/6; -pi/6; -pi/2; 0; 120*pi/180];
+q_fin3 = [pi/2; pi/3; -pi/2; pi/4; -150*pi/180; -110*pi/180; 100*pi/180];
+q_fin4 = [pi/2; -pi/3; -120*pi/180; -pi/6; pi/3; 110*pi/180; -70*pi/180];
+q_fin5 = [-pi/4; 70*pi/180; -pi/4; 0; -150*pi/180; pi/2; -45*pi/180];
+q_fin6 = [-160*pi/180; -70*pi/180; pi/2; -100*pi/180; 130*pi/180; -69*pi/180; -100*pi/180];  
+q_fin7 = [-pi/2;0;130*pi/180;pi/4;0;-pi/2;0];
+
+t = 0:deltaT:T;
+
+%Trajectory 1
+for i=1:N
+    a = -2/(t_step^3) * (q_fin1(i)-q_0(i));
+    b = 3/(t_step^2) * (q_fin1(i)-q_0(i));
+    for j=1:t_step/deltaT
+        TRAJ1(i,j) = a*t(j)^3 + b*t(j)^2 + q_0(i);
+    end
+end
+
+
+
+%Trajectory 2
+for i=1:N
+    a = -2/(t_step^3) * (q_fin2(i)-q_fin1(i));
+    b = 3/(t_step^2) * (q_fin2(i)-q_fin1(i));
+    for j=1:t_step/deltaT
+        TRAJ2(i,j) = a*t(j)^3 + b*t(j)^2 + q_fin1(i);
+    end
+end
+
+%Trajectory 3
+for i=1:N
+    a = -2/(t_step^3) * (q_fin3(i)-q_fin2(i));
+    b = 3/(t_step^2) * (q_fin3(i)-q_fin2(i));
+    for j=1:t_step/deltaT
+        TRAJ3(i,j) = a*t(j)^3 + b*t(j)^2 + q_fin2(i);
+    end
+end
+
+%Trajectory 4
+for i=1:N
+    a = -2/(t_step^3) * (q_fin4(i)-q_fin3(i));
+    b = 3/(t_step^2) * (q_fin4(i)-q_fin3(i));
+    for j=1:t_step/deltaT
+        TRAJ4(i,j) = a*t(j)^3 + b*t(j)^2 + q_fin3(i);
+    end
+end
+
+%Trajectory 5
+for i=1:N
+    a = -2/(t_step^3) * (q_fin5(i)-q_fin4(i));
+    b = 3/(t_step^2) * (q_fin5(i)-q_fin4(i));
+    for j=1:t_step/deltaT
+        TRAJ5(i,j) = a*t(j)^3 + b*t(j)^2 + q_fin4(i);
+    end
+end
+
+%Trajectory 6
+for i=1:N
+    a = -2/(t_step^3) * (q_fin6(i)-q_fin5(i));
+    b = 3/(t_step^2) * (q_fin6(i)-q_fin5(i));
+    for j=1:t_step/deltaT
+        TRAJ6(i,j) = a*t(j)^3 + b*t(j)^2 + q_fin5(i);
+    end
+end
+
+%Last Trajectory
+for i=1:N
+    a = -2/(t_step^3) * (q_fin7(i)-q_fin6(i));
+    b = 3/(t_step^2) * (q_fin7(i)-q_fin6(i));
+    for j=1:t_step/deltaT+1
+        TRAJ7(i,j) = a*t(j)^3 + b*t(j)^2 + q_fin6(i);
+    end
+end
+
+%Building the whole trajectory
+q = [TRAJ1, TRAJ2, TRAJ3, TRAJ4, TRAJ5, TRAJ6, TRAJ7];
+
+%Building dq and ddq via differentiation
+dq = TimeDerivative(q);
+ddq = TimeDerivative(dq);
+
+%Useful plots for verifying feasibility
+figure
+for i=1:7
+    subplot(4,2,i)
+    plot(t,q(i,:),'b',t,q_lim(i)*ones(size(t)),'--',...
+        t,-q_lim(i)*ones(size(t)),'--');
+    xlabel('time [s]');
+    ylabelstring = sprintf('q_%i [rad]',i);
+    ylabel(ylabelstring);
+    title("Positions");
+    ylim([-3.2 3.2])
+    grid
+end
+
+figure
+for i=1:7
+    subplot(4,2,i)
+    plot(t(1:end-1),dq(i,:),t,qd_lim(i)*ones(size(t)),...
+        t,-qd_lim(i)*ones(size(t)));
+    xlabel('time [s]');
+    ylabelstring = sprintf('dq_%i [rad/s]',i);
+    ylabel(ylabelstring);
+    title("Velocities");
+    grid
+end
+
+%Print the trajectories on files for VREP and for the FRI
+fVREP = fopen(nameVREP, 'wt');
+fDEF = fopen(nameDEF, 'wt');
+fCPP = fopen(nameCPP, 'wt');
+dim = T/deltaT + 1;
+for i=1:dim
+    fprintf(fVREP, '%f %f %f %f %f %f %f %f\n', t(i), -q(1, i), q(2,i), q(3,i), q(4,i), -q(5,i), q(6,i), q(7,i));   
+end
+
+for j=1:N
+    for i=1:samples
+        fprintf(fCPP, '%f\t', q(j,i));
+    end
+    fprintf(fCPP, '\n');
+end 
+fprintf(fDEF, 'T = 12s, t_step = 2, deltaT = 0.005;');
+
+fclose(fVREP);
+fclose(fCPP);
+fclose(fDEF);
+cd('../')

Coefficients Identification/Gen_periodic.m

@@ -0,0 +1,86 @@
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%File for generating periodic trajectory%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+close all
+clear all
+
+nameDEF = 'q_sin1_def.txt';
+nameVREP = 'q_sin1_vrep.txt';
+nameCPP = 'q_sin1_cpp.txt';
+
+
+%Global parameters of the trajectory
+T = 20;
+deltaT = 0.005;
+N = 7;
+t1 = 0:deltaT:T;
+samples = T/deltaT;
+
+%Joint limits
+q_lim = [170 120 170 120 170 120 170]*pi/180;   %rad
+qd_lim = [110, 110, 128, 128, 204, 184, 184]*pi/180;  %rad/s
+
+data = [];
+for t = 0:deltaT:T
+    q = [1.4*sin(pi/4*t) 1.4*sin(pi/5*t) 1.8*sin(pi/6*t) 1.3*sin(pi/4*t-pi/6) 3*sin(pi/3*t) 2*sin(pi/6*t) 3*sin(pi/5*t)];
+    %Control on joint limits
+	for i=1:N
+        if q(i) > q_lim(i)
+            q(i) = q_lim(i);
+        elseif q(i) < -q_lim(i)
+            q(i) = -q_lim(i);
+        end
+    end
+    data = [data; t, q];
+end
+
+%Building derivatives via differentiation
+dq = TimeDerivative(q);
+ddq = TimeDerivative(dq);
+
+%Useful plots for validating feasibility
+figure
+for i=2:8
+    subplot(4,2,i-1)
+    plot(t1,data(:,i));
+    xlabel('time [s]');
+    ylabelstring = sprintf('q_%i [rad]',i-1);
+    ylabel(ylabelstring);
+    title("Positions");
+    ylim([-2.9671 2.9671])
+    grid
+end
+
+figure
+for i=1:7
+    subplot(4,2,i)
+    plot(t1(1:end-1),dq(i,:));
+    xlabel('time [s]');
+    ylabelstring = sprintf('dq_%i [rad/s]',i);
+    ylabel(ylabelstring);
+    title("Velocities");
+    grid
+end
+
+%Print the trajectories on files for VREP and for the FRI
+fVREP = fopen(nameVREP, 'wt');
+fDEF = fopen(nameDEF, 'wt');
+fCPP = fopen(nameCPP, 'wt');
+dim = T/deltaT + 1;
+for i=1:dim
+    fprintf(fVREP, '%f %f %f %f %f %f %f %f\n', data(i,1), -data(i, 2), data(i,3),...
+        data(i,4), data(i,5), -data(i,6), data(i,7), data(i,8));   
+end
+
+for j=2:8
+    for i=1:dim
+        fprintf(fCPP, '%f\t', data(i,j));
+    end
+    fprintf(fCPP, '\n');
+end 
+fprintf(fDEF, 'T = 20s, t = 0.005, different amplitudes and pulses');
+
+fclose(fVREP);
+fclose(fCPP);
+fclose(fDEF);

Coefficients Identification/NoncausalButterworthFilter.m

@@ -0,0 +1,19 @@
+%%%%%%%%%%%%
+% NEW CODE %
+%%%%%%%%%%%%
+function Matrix_filt = NoncausalButterworthFilter(Matrix)
+% The function returns the filtered version of Matrix.
+% Matrix collects the time history of a vector (it is a thin matrix). 
+% Its rows are the samples, its colums are different components of the 
+% vector.
+
+[num_of_components, ~] = size(Matrix);
+
+b = ones(1,100)/100;
+a = 1;
+for j = 1 : num_of_components
+    Matrix_filt(j,:) = filtfilt(b,a,Matrix(j,:)); 
+end
+
+end
+

Coefficients Identification/TimeDerivative.m

@@ -0,0 +1,19 @@
+%%%%%%%%%%%%
+% NEW CODE %
+%%%%%%%%%%%%
+function MatrixD_filt = TimeDerivative(Matrix, deltaTime)
+% It returns the filtered version of the incremential retio of matrix.
+% Matrix collects the time history of a vector (it is a thin matrix). 
+% Its rows are the samples, its colums are different components of the 
+% vector.
+[num_of_components, num_of_samples] = size(Matrix);
+MatrixD = zeros(num_of_components, num_of_samples);
+MatrixD_filt = zeros(num_of_components, num_of_samples);
+
+for j = 1 : num_of_components
+    for i = 1:num_of_samples-1
+        MatrixD(j,i) = (Matrix(j,i+1)-Matrix(j,i))/deltaTime;
+    end    
+    MatrixD_filt(j,:) = NoncausalButterworthFilter(MatrixD(j,:));   
+end
+end

Coefficients Identification/coeff_validation.m

@@ -0,0 +1,71 @@
+%%%%%%%%%%%%
+%VALIDATION%
+%%%%%%%%%%%%
+
+%In this file we evaluate the identified parameters on a test trajectory
+
+
+%Definitions
+N=7;
+deltat = 0.005;
+d3 = 0.4;
+d5 = 0.39;
+
+
+q = dlmread('lab_traj/TEST/positions.txt');
+
+samples = length(q)/N;
+temp = zeros(N,samples);
+
+%Reordering data into a [#joints,#samples] matrix
+for i=1:samples
+    temp(:,i) = q(N*(i-1)+1:N*(i-1)+N);
+end
+q = temp;
+
+
+tau = dlmread('lab_traj/TEST/torquesvector.txt');
+
+
+temp = zeros(N,samples);
+
+%Reordering data into a [#joints,#samples] matrix
+for i=1:samples
+    temp(:,i) = tau(N*(i-1)+1:N*(i-1)+N);
+end
+tau = temp;
+
+%Reading the identified parameters	
+coeff = dlmread('greekPi.txt');
+
+
+%Build q, dq, ddq and filter
+qFILT = NoncasualButterworthFilter(q);
+dq = TimeDerivative(q);
+dqFILT = NoncasualButterworthFilter(dq);
+ddq = TimeDerivative(dq);
+ddqFILT = NoncasualButterworthFilter(ddq);
+tauFILT = NoncasualButterworthFilter(tau);
+
+%Evaluating the new estimation of torques
+tau_hat = zeros(size(q));
+for i=1:samples
+    q = qFILT(:,i);
+    dq = dqFILT(:,i);
+    ddq = ddqFILT(:,i);
+    Y = Regressor(q,dq,ddq,d3,d5);
+    tau_hat(:, i) = Y*coeff;
+end
+
+
+t=0:deltat:samples*deltat;
+
+%Plot of the results
+figure
+for i=1:7
+    subplot(4,2,i)
+    plot(t(1:end-1),tauFILT(i,:), t(1:end-1), tau_hat(i,:), 'LineWidth', 2);
+    grid
+    xlabel('Time [s]');
+    ylabel(sprintf('$\\tau_%i$ [Nm]', i), 'Interpreter', 'latex');
+end