128 lines
4.7 KiB
Matlab
128 lines
4.7 KiB
Matlab
% clc; clear all; close all;
|
|
|
|
% ------------------------------------------------------------------------
|
|
% Load and calculate desired trajectory
|
|
% ------------------------------------------------------------------------
|
|
load('ptrnSrch_N5T20QR.mat');
|
|
[q,qd,q2d] = mixed_traj(traj_par.t,c_pol,a,b,traj_par.wf,traj_par.N);
|
|
|
|
|
|
% ------------------------------------------------------------------------
|
|
% Load the real trajectory of the robot without load
|
|
% ------------------------------------------------------------------------
|
|
traj_data = csvread('ur-19_12_23_free.csv');
|
|
|
|
int_idx = 1; % get the point when the trajectory begins
|
|
fnl_idx = 2036;
|
|
|
|
unloadedTrajectory = struct; % structure with trajectory data
|
|
|
|
unloadedTrajectory.t = traj_data(int_idx:fnl_idx,1) - traj_data(int_idx,1);
|
|
unloadedTrajectory.q = traj_data(int_idx:fnl_idx,2:7);
|
|
unloadedTrajectory.qd = traj_data(int_idx:fnl_idx,8:13);
|
|
unloadedTrajectory.i = traj_data(int_idx:fnl_idx,14:19);
|
|
unloadedTrajectory.i_des = traj_data(int_idx:fnl_idx,20:25);
|
|
unloadedTrajectory.tau_des = traj_data(int_idx:fnl_idx,26:31);
|
|
|
|
|
|
% -----------------------------------------------------------------------
|
|
% Load the real trajectory of the robot with load
|
|
% -----------------------------------------------------------------------
|
|
% traj_ldd = csvread('ur-19_12_23_load.csv'); % load 1 kg
|
|
traj_ldd = csvread('ur-20_01_13-load_2600.csv'); % load 2.6 kg
|
|
|
|
int_idx_ldd = 250;
|
|
fnl_idx_ldd = 2274;
|
|
|
|
loadedTrajectory = struct;
|
|
|
|
loadedTrajectory.t = traj_ldd(int_idx_ldd:fnl_idx_ldd,1) - traj_ldd(int_idx_ldd,1);
|
|
loadedTrajectory.q = traj_ldd(int_idx_ldd:fnl_idx_ldd,2:7);
|
|
loadedTrajectory.qd = traj_ldd(int_idx_ldd:fnl_idx_ldd,8:13);
|
|
loadedTrajectory.i = traj_ldd(int_idx_ldd:fnl_idx_ldd,14:19);
|
|
loadedTrajectory.i_des = traj_ldd(int_idx_ldd:fnl_idx_ldd,20:25);
|
|
loadedTrajectory.tau_des = traj_ldd(int_idx_ldd:fnl_idx_ldd,26:31);
|
|
|
|
|
|
% ------------------------------------------------------------------------
|
|
% Filtering Velocities
|
|
% ------------------------------------------------------------------------
|
|
% Design filter
|
|
vel_filt = designfilt('lowpassiir','FilterOrder',3, ...
|
|
'HalfPowerFrequency',0.2,'DesignMethod','butter');
|
|
|
|
unloadedTrajectory.qd_fltrd = zeros(size(unloadedTrajectory.qd));
|
|
for i = 1:6
|
|
unloadedTrajectory.qd_fltrd(:,i) = filtfilt(vel_filt,unloadedTrajectory.qd(:,i));
|
|
end
|
|
|
|
loadedTrajectory.qd_fltrd = zeros(size(loadedTrajectory.qd));
|
|
for i = 1:6
|
|
loadedTrajectory.qd_fltrd(:,i) = filtfilt(vel_filt,loadedTrajectory.qd(:,i));
|
|
end
|
|
|
|
|
|
% ------------------------------------------------------------------------
|
|
% Estimating accelerations
|
|
% ------------------------------------------------------------------------
|
|
% Three point central difference
|
|
unloadedTrajectory.q2d_est = zeros(size(unloadedTrajectory.qd_fltrd));
|
|
for i = 2:length(unloadedTrajectory.qd_fltrd)-1
|
|
dlta_qd_fltrd = unloadedTrajectory.qd_fltrd(i+1,:) - ...
|
|
unloadedTrajectory.qd_fltrd(i-1,:);
|
|
dlta_t_msrd = unloadedTrajectory.t(i+1) - unloadedTrajectory.t(i-1);
|
|
unloadedTrajectory.q2d_est(i,:) = dlta_qd_fltrd/dlta_t_msrd;
|
|
end
|
|
|
|
loadedTrajectory.q2d_est = zeros(size(loadedTrajectory.qd_fltrd));
|
|
for i = 2:length(loadedTrajectory.qd_fltrd)-1
|
|
dlta_qd_fltrd_ldd = loadedTrajectory.qd_fltrd(i+1,:) - ...
|
|
loadedTrajectory.qd_fltrd(i-1,:);
|
|
dlta_t_msrd_ldd = loadedTrajectory.t(i+1) - loadedTrajectory.t(i-1);
|
|
loadedTrajectory.q2d_est(i,:) = dlta_qd_fltrd_ldd/dlta_t_msrd_ldd;
|
|
end
|
|
|
|
% Zeros phase filtering acceleration obtained by finite difference
|
|
accel_filt = designfilt('lowpassiir','FilterOrder',5, ...
|
|
'HalfPowerFrequency',0.05,'DesignMethod','butter');
|
|
for i = 1:6
|
|
unloadedTrajectory.q2d_est(:,i) = filtfilt(accel_filt,unloadedTrajectory.q2d_est(:,i));
|
|
end
|
|
|
|
for i = 1:6
|
|
loadedTrajectory.q2d_est(:,i) = filtfilt(accel_filt,loadedTrajectory.q2d_est(:,i));
|
|
end
|
|
|
|
|
|
% ------------------------------------------------------------------------
|
|
% Filtering current
|
|
% ------------------------------------------------------------------------
|
|
% Zeros phase filtering acceleration obtained by finite difference
|
|
curr_filt = designfilt('lowpassiir','FilterOrder',5, ...
|
|
'HalfPowerFrequency',0.1,'DesignMethod','butter');
|
|
|
|
for i = 1:6
|
|
unloadedTrajectory.i_fltrd(:,i) = filtfilt(curr_filt,unloadedTrajectory.i(:,i));
|
|
end
|
|
|
|
for i = 1:6
|
|
loadedTrajectory.i_fltrd(:,i) = filtfilt(curr_filt,loadedTrajectory.i(:,i));
|
|
end
|
|
|
|
|
|
%% ----------------------------
|
|
%{
|
|
noJoint = 1;
|
|
|
|
figure
|
|
hold on
|
|
plot(unloadedTrajectory.t, unloadedTrajectory.i(:,noJoint))
|
|
plot(unloadedTrajectory.t, unloadedTrajectory.i_fltrd(:,noJoint),'LineWidth',2)
|
|
plot(loadedTrajectory.t, loadedTrajectory.i(:,noJoint))
|
|
plot(loadedTrajectory.t, loadedTrajectory.i_fltrd(:,noJoint),'LineWidth',2)
|
|
%}
|
|
|
|
|
|
|
|
|