Robotics
/
Dynamic-Calibration
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

indeitification of UR is changed and writeen in terms of functions in order to ease comparison of different data and drive gains. Pendubot indentification is changed to include validation with parameters from CAD

This commit is contained in:
Shamil Mamedov 2020-02-07 15:07:15 +03:00
parent fe3cc01102
commit 40df589ea7
37 changed files with 12287 additions and 303 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3104
data_ur10/ur-20_02_05-20sec_12harm.csv Normal file
View File

File diff suppressed because it is too large Load Diff

2506
data_ur10/ur-20_02_05-20sec_8harm.csv Normal file
View File

File diff suppressed because it is too large Load Diff

3354
data_ur10/ur-20_02_06-20sec_10harm.csv Normal file
View File

File diff suppressed because it is too large Load Diff

2910
data_ur10/ur-20_02_06-20sec_5harm.csv Normal file
View File

File diff suppressed because it is too large Load Diff

34
functions/filterData.m
View File

@ -12,12 +12,14 @@ function data = filterData(data)
% filtered currents, data.q2d_est - estimated accelerations % filtered currents, data.q2d_est - estimated accelerations
% --------------------------------------------------------------------- % ---------------------------------------------------------------------
% data = parseURData('ur-20_02_05-20sec_8harm.csv', 320, 2310);
% --------------------------------------------------------------------- % ---------------------------------------------------------------------
% Filtering Velocities % Filtering Velocities
% --------------------------------------------------------------------- % ---------------------------------------------------------------------
% Design filter % Design filter
vel_filt = designfilt('lowpassiir','FilterOrder',3, ... vel_filt = designfilt('lowpassiir','FilterOrder',5, ...
'HalfPowerFrequency',0.2,'DesignMethod','butter'); 'HalfPowerFrequency',0.15,'DesignMethod','butter');
data.qd_fltrd = zeros(size(data.qd)); data.qd_fltrd = zeros(size(data.qd));
for i = 1:6 for i = 1:6
@ -38,7 +40,7 @@ end
% Zeros phase filtering acceleration obtained by finite difference % Zeros phase filtering acceleration obtained by finite difference
accel_filt = designfilt('lowpassiir','FilterOrder',5, ... accel_filt = designfilt('lowpassiir','FilterOrder',5, ...
'HalfPowerFrequency',0.05,'DesignMethod','butter'); 'HalfPowerFrequency',0.15,'DesignMethod','butter');
for i = 1:6 for i = 1:6
data.q2d_est(:,i) = filtfilt(accel_filt,data.q2d_est(:,i)); data.q2d_est(:,i) = filtfilt(accel_filt,data.q2d_est(:,i));
end end
@ -48,7 +50,7 @@ end
% ------------------------------------------------------------------------ % ------------------------------------------------------------------------
% Zeros phase filtering acceleration obtained by finite difference % Zeros phase filtering acceleration obtained by finite difference
curr_filt = designfilt('lowpassiir','FilterOrder',5, ... curr_filt = designfilt('lowpassiir','FilterOrder',5, ...
'HalfPowerFrequency',0.1,'DesignMethod','butter'); 'HalfPowerFrequency',0.20,'DesignMethod','butter');
for i = 1:6 for i = 1:6
data.i_fltrd(:,i) = filtfilt(curr_filt,data.i(:,i)); data.i_fltrd(:,i) = filtfilt(curr_filt,data.i(:,i));
@ -64,3 +66,27 @@ end
for i = 1:6 for i = 1:6
data.tau_des_fltrd(:,i) = filtfilt(curr_filt,data.tau_des(:,i)); data.tau_des_fltrd(:,i) = filtfilt(curr_filt,data.tau_des(:,i));
end end
% Functions for plotting
function plotVelocity(trj)
figure
plot(trj.t, trj.qd)
hold on
plot(trj.t, trj.qd_fltrd)
end
function plotCurrent(trj)
figure
plot(trj.t, trj.i)
hold on
plot(trj.t, trj.i_fltrd)
end
function plotAcceleration(trj)
figure
plot(trj.t, trj.q2d_est)
end
end

3
planar2DOF/bags_pndbt.m
View File

@ -1,6 +1,7 @@
clc; clear all; clc; clear all;
name = 'current_A_0.7_v_1.bag'; % name = 'position_A_0.7_v_0.5.bag';
name = 'position_A_1.2_v_0.05.bag';
bag = rosbag(name); bag = rosbag(name);
end_time = 20; end_time = 20;

BIN
planar2DOF/data_pndbt/current_A_0.5_v_1.bag
View File

Binary file not shown.

BIN
planar2DOF/data_pndbt/current_A_0.7_v_0.5.bag
View File

Binary file not shown.

BIN
planar2DOF/data_pndbt/current_A_0.7_v_1.bag
View File

Binary file not shown.

BIN
planar2DOF/data_pndbt/current_A_0.7_v_1.mat
View File

Binary file not shown.

BIN
planar2DOF/data_pndbt/current_A_5_v_4.bag
View File

Binary file not shown.

BIN
planar2DOF/data_pndbt/current_A_5_v_4.mat
View File

Binary file not shown.

BIN
planar2DOF/data_pndbt/position_A_0.7_v_0.5.bag Normal file
View File

Binary file not shown.

BIN
planar2DOF/data_pndbt/position_A_0.7_v_0.5.mat Normal file
View File

Binary file not shown.

BIN
planar2DOF/data_pndbt/position_A_0.7_v_1.0.bag Normal file
View File

Binary file not shown.

BIN
planar2DOF/data_pndbt/position_A_0.7_v_1.0.mat Normal file
View File

Binary file not shown.

BIN
planar2DOF/data_pndbt/position_A_1.2_v_0.05.bag Normal file
View File

Binary file not shown.

BIN
planar2DOF/data_pndbt/position_A_1.2_v_0.05.mat Normal file
View File

Binary file not shown.

11
planar2DOF/full_regressor_plnr.m
View File

@ -3,7 +3,7 @@ function Y = full_regressor_plnr(in1,in2,in3)
% Y = FULL_REGRESSOR_PLNR(IN1,IN2,IN3) % Y = FULL_REGRESSOR_PLNR(IN1,IN2,IN3)
% This function was generated by the Symbolic Math Toolbox version 8.2. % This function was generated by the Symbolic Math Toolbox version 8.2.
% 03-Feb-2020 10:03:33 % 07-Feb-2020 10:59:41
q1 = in1(1,:); q1 = in1(1,:);
q2 = in1(2,:); q2 = in1(2,:);
@ -17,8 +17,7 @@ t4 = sin(q2);
t5 = cos(q1); t5 = cos(q1);
t6 = sin(q1); t6 = sin(q1);
t7 = qd2.^2; t7 = qd2.^2;
t8 = t2.*t5.*(9.81e2./1.0e2); t8 = q2d1+q2d2;
t9 = q2d1+q2d2; t9 = t2.*t3.*t5.*(9.81e2./1.0e2);
t10 = t2.*t3.*t5.*(9.81e2./1.0e2); t10 = qd1.^2;
t11 = qd1.^2; Y = reshape([0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,q2d1,0.0,t2.*t5.*(9.81e2./1.0e2),0.0,t2.*t6.*(-9.81e2./1.0e2),0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t8,t8,t9+(q2d1.*t3)./2.0+(q2d2.*t3)./4.0-(t4.*t7)./4.0-(qd1.*qd2.*t4)./2.0-t2.*t4.*t6.*(9.81e2./1.0e2),t9+(q2d1.*t3)./4.0+(t4.*t10)./4.0-t2.*t4.*t6.*(9.81e2./1.0e2),q2d1.*t4.*(-1.0./2.0)-(q2d2.*t4)./4.0-(t3.*t7)./4.0-(qd1.*qd2.*t3)./2.0-t2.*t3.*t6.*(9.81e2./1.0e2)-t2.*t4.*t5.*(9.81e2./1.0e2),q2d1.*t4.*(-1.0./4.0)+(t3.*t10)./4.0-t2.*t3.*t6.*(9.81e2./1.0e2)-t2.*t4.*t5.*(9.81e2./1.0e2),0.0,0.0,q2d1./1.6e1+t2.*t5.*(9.81e2./4.0e2),0.0],[2,20]);
Y = reshape([0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,q2d1,0.0,t8,0.0,t2.*t6.*(-9.81e2./1.0e2),0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t9,t9,t10+q2d1.*t3.*2.0+q2d2.*t3-t4.*t7-qd1.*qd2.*t4.*2.0-t2.*t4.*t6.*(9.81e2./1.0e2),t10+q2d1.*t3+t4.*t11-t2.*t4.*t6.*(9.81e2./1.0e2),q2d1.*t4.*-2.0-q2d2.*t4-t3.*t7-qd1.*qd2.*t3.*2.0-t2.*t3.*t6.*(9.81e2./1.0e2)-t2.*t4.*t5.*(9.81e2./1.0e2),-q2d1.*t4+t3.*t11-t2.*t3.*t6.*(9.81e2./1.0e2)-t2.*t4.*t5.*(9.81e2./1.0e2),0.0,0.0,q2d1+t8,0.0],[2,20]);

10
planar2DOF/planar_manip.urdf
View File

@ -14,10 +14,10 @@
<link name = "link 1"> <link name = "link 1">
<inertial> <inertial>
<origin xyz = "0.125 0 0" <origin xyz = "0.043 0 0"
rpy = "0 0 0"/> rpy = "0 0 0"/>
<mass value = "0.29" /> <mass value = "1.085" />
<inertia ixx = "1" iyy = "1" izz = "0.5" <inertia ixx = "1" iyy = "1" izz = "0.008"
ixy = "0.01" ixz = "0.01" iyz = "0.01" /> ixy = "0.01" ixz = "0.01" iyz = "0.01" />
</inertial> </inertial>
<visual> <visual>
@ -34,10 +34,10 @@
<link name = "link 2"> <link name = "link 2">
<inertial> <inertial>
<origin xyz = "0.125 0 0" <origin xyz = "0.0954 0 0"
rpy = "0 0 0"/> rpy = "0 0 0"/>
<mass value = "0.26" /> <mass value = "0.26" />
<inertia ixx = "1" iyy = "1" izz = "0.3" <inertia ixx = "1" iyy = "1" izz = "0.002"
ixy = "0.01" ixz = "0.01" iyz = "0.01" /> ixy = "0.01" ixz = "0.01" iyz = "0.01" />
</inertial> </inertial>
<visual> <visual>

BIN
planar2DOF/plnrBaseQR.mat
View File

Binary file not shown.

5
planar2DOF/plnr_baseQR.m
View File

@ -9,8 +9,8 @@ includeMotorDynamics = 1;
% limits on positions, velocities, accelerations % limits on positions, velocities, accelerations
q_min = -deg2rad(160); q_min = -2*pi;
q_max = 0; q_max = 2*pi;
qd_min = -10; qd_min = -10;
qd_max = 10; qd_max = 10;
q2d_min = -100; q2d_min = -100;
@ -97,7 +97,6 @@ pi_lgr_base = pi1 + beta*pi2;
pi_lgr_base2 = [eye(bb) beta]*[pi1;pi2]; pi_lgr_base2 = [eye(bb) beta]*[pi1;pi2];
pi_lgr_base3 = [eye(bb) beta]*E'*pi_pndbt_sym; pi_lgr_base3 = [eye(bb) beta]*E'*pi_pndbt_sym;
% ----------------------------------------------------------------------- % -----------------------------------------------------------------------
% Validation of obtained mappings % Validation of obtained mappings
% ----------------------------------------------------------------------- % -----------------------------------------------------------------------

2
planar2DOF/plnr_idntfcn.m
View File

@ -32,7 +32,7 @@ for i = 1:2
plnr.h(:,i) = link_mass*com_pos; plnr.h(:,i) = link_mass*com_pos;
plnr.I_vec(:,i) = inertiaMatrix2Vector(link_inertia-... plnr.I_vec(:,i) = inertiaMatrix2Vector(link_inertia-...
link_mass*com_vec2mat*com_vec2mat); link_mass*com_vec2mat*com_vec2mat);
plnr.pi(:,i) = [plnr.I_vec(:,i);plnr.h(:,i);plnr.m(i)]; plnr.pi(:,i) = [plnr.I_vec(:,i); plnr.h(:,i); plnr.m(i)];
end end
return return

3
planar2DOF/plnr_regressor.m
View File

@ -71,11 +71,12 @@ for i = 1:noIter
Ylgr = Y_fcn(q,qd,q2d); Ylgr = Y_fcn(q,qd,q2d);
tau1 = inverseDynamics(rbt,q,qd,q2d); tau1 = inverseDynamics(rbt,q,qd,q2d);
tau2 = Ylgr*reshape(plnr.pi,[20,1]); tau2 = Ylgr*plnr.pi(:);
% verifying if regressor is computed correctly % verifying if regressor is computed correctly
err1(i) = norm(tau1 - tau2); err1(i) = norm(tau1 - tau2);
end end
plot(err1)
if all(err1<1e-6) if all(err1<1e-6)
disp('Regressor matrix is computed correctly!') disp('Regressor matrix is computed correctly!')

16
planar2DOF/pndbt_data_processing.m
View File

@ -3,8 +3,8 @@
run('plnr_idntfcn.m') run('plnr_idntfcn.m')
% load pendubot data % load pendubot data
% rawData = load('current_A_5_v_4.mat'); % rawData = load('position_A_0.7_v_0.5.mat');
rawData = load('current_A_0.7_v_1.mat'); rawData = load('position_A_1.2_v_0.05.mat');
% parse pendubot data % parse pendubot data
pendubot.time = rawData.data(:,1) - rawData.data(1,1); pendubot.time = rawData.data(:,1) - rawData.data(1,1);
@ -17,9 +17,6 @@ pendubot.shldr_velocity = rawData.data(:,9);
pendubot.elbw_position = rawData.data(:,8); pendubot.elbw_position = rawData.data(:,8);
pendubot.elbw_velocity = rawData.data(:,10); pendubot.elbw_velocity = rawData.data(:,10);
pendubot.position_desired = rawData.data(:,5);
pendubot.velocity_desired = rawData.data(:,6);
% filter velocties with zero phas filter % filter velocties with zero phas filter
vlcty_fltr = designfilt('lowpassiir','FilterOrder',5, ... vlcty_fltr = designfilt('lowpassiir','FilterOrder',5, ...
'HalfPowerFrequency',0.25,'DesignMethod','butter'); 'HalfPowerFrequency',0.25,'DesignMethod','butter');
@ -50,6 +47,12 @@ trq_fltr = designfilt('lowpassiir','FilterOrder',5, ...
pendubot.torque_filtered = filtfilt(trq_fltr, pendubot.torque); pendubot.torque_filtered = filtfilt(trq_fltr, pendubot.torque);
%% %%
% figure
% plot(pendubot.time, pendubot.current*0.123)
% hold on
% plot(pendubot.time, pendubot.torque)
return return
plotJointPositions(pendubot) plotJointPositions(pendubot)
plotJointVelocities(pendubot) plotJointVelocities(pendubot)
@ -64,10 +67,13 @@ function plotJointPositions(pendubot)
figure figure
subplot(2,1,1) subplot(2,1,1)
plot(pendubot.time, pendubot.shldr_position) plot(pendubot.time, pendubot.shldr_position)
hold on
plot(pendubot.time, pendubot.current_desired)
xlim([0 1]) xlim([0 1])
xlabel('$t$, sec','interpreter', 'latex') xlabel('$t$, sec','interpreter', 'latex')
ylabel('$q_1$, rad','interpreter', 'latex') ylabel('$q_1$, rad','interpreter', 'latex')
grid on grid on
legend('measured', 'desired')
subplot(2,1,2) subplot(2,1,2)
plot(pendubot.time, pendubot.elbw_position) plot(pendubot.time, pendubot.elbw_position)
xlim([0 1]) xlim([0 1])

15
planar2DOF/pndbt_idntfcn.m
View File

@ -7,7 +7,7 @@ load('plnrBaseQR.mat')
% compose observation matrix and torque vector % compose observation matrix and torque vector
noObservations = length(pendubot.time); noObservations = length(pendubot.time);
W = []; Tau = []; W = []; Tau = [];
for i = 1:noObservations for i = 1:1:noObservations
qi = [pendubot.shldr_position(i), pendubot.elbw_position(i)]'; qi = [pendubot.shldr_position(i), pendubot.elbw_position(i)]';
qdi = [pendubot.shldr_velocity_filtered(i), pendubot.elbw_velocity_filtered(i)]'; qdi = [pendubot.shldr_velocity_filtered(i), pendubot.elbw_velocity_filtered(i)]';
q2di = [pendubot.shldr_acceleration_filtered(i), pendubot.elbow_acceleration_filtered(i)]'; q2di = [pendubot.shldr_acceleration_filtered(i), pendubot.elbow_acceleration_filtered(i)]';
@ -21,7 +21,8 @@ for i = 1:noObservations
Yfrctni = frictionRegressor(qdi); Yfrctni = frictionRegressor(qdi);
W = vertcat(W, [Ybi, Yfrctni]); W = vertcat(W, [Ybi, Yfrctni]);
taui = [pendubot.torque(i), 0]'; taui = [-pendubot.torque(i), 0]';
% taui = [pendubot.current(i)*0.123, 0]';
Tau = vertcat(Tau, taui); Tau = vertcat(Tau, taui);
end end
@ -31,7 +32,7 @@ pi_hat = (W'*W)\(W'*Tau)
%% Set-up SDP optimization procedure %% Set-up SDP optimization procedure
physicalConsistency = 1; physicalConsistency = 0;
pi_frctn = sdpvar(6,1); pi_frctn = sdpvar(6,1);
pi_b = sdpvar(plnrBaseQR.numberOfBaseParameters,1); % variables for base paramters pi_b = sdpvar(plnrBaseQR.numberOfBaseParameters,1); % variables for base paramters
@ -70,10 +71,10 @@ else
end end
cnstr = [cnstr, pii(11) > 0]; % first motor inertia constraint cnstr = [cnstr, pii(11) > 0]; % first motor inertia constraint
% Feasibility constraints on the friction prameters % % Feasibility constraints on the friction prameters
for i = 1:2 % for i = 1:2
cnstr = [cnstr, pi_frctn(3*i-2)>0, pi_frctn(3*i-1)>0]; % cnstr = [cnstr, pi_frctn(3*i-2)>0, pi_frctn(3*i-1)>0];
end % end
% Defining pbjective function % Defining pbjective function
obj = norm(Tau - W*[pi_b; pi_frctn], 2); obj = norm(Tau - W*[pi_b; pi_frctn], 2);

21
planar2DOF/pndbt_vldtn.m
View File

@ -1,6 +1,6 @@
% load pendubot data % load pendubot data
% rawData = load('current_A_0.7_v_1.mat'); rawData = load('position_A_0.7_v_1.0.mat');
rawData = load('current_A_5_v_4.mat'); % rawData = load('position_A_1.2_v_0.05.mat');
% parse pendubot data % parse pendubot data
pendubot.time = rawData.data(:,1) - rawData.data(1,1); pendubot.time = rawData.data(:,1) - rawData.data(1,1);
@ -36,7 +36,7 @@ pendubot.elbow_acceleration = q2d2;
% filter estimated accelerations with zero phase filter % filter estimated accelerations with zero phase filter
aclrtn_fltr = designfilt('lowpassiir','FilterOrder',5, ... aclrtn_fltr = designfilt('lowpassiir','FilterOrder',5, ...
'HalfPowerFrequency',0.25,'DesignMethod','butter'); 'HalfPowerFrequency',0.1,'DesignMethod','butter');
pendubot.shldr_acceleration_filtered = filtfilt(aclrtn_fltr, pendubot.shldr_acceleration); pendubot.shldr_acceleration_filtered = filtfilt(aclrtn_fltr, pendubot.shldr_acceleration);
pendubot.elbow_acceleration_filtered = filtfilt(aclrtn_fltr, pendubot.elbow_acceleration); pendubot.elbow_acceleration_filtered = filtfilt(aclrtn_fltr, pendubot.elbow_acceleration);
@ -45,10 +45,12 @@ trq_fltr = designfilt('lowpassiir','FilterOrder',5, ...
'HalfPowerFrequency',0.2,'DesignMethod','butter'); 'HalfPowerFrequency',0.2,'DesignMethod','butter');
pendubot.torque_filtered = filtfilt(trq_fltr, pendubot.torque); pendubot.torque_filtered = filtfilt(trq_fltr, pendubot.torque);
%%
% Validation % Validation
vldtnRange = 1:200; pi_CAD = [plnr.pi(:,1); 0; plnr.pi(:,2)];
tau_prdctd_OLS = []; tau_prdctd_SDP = []; vldtnRange = 1:1500;
tau_prdctd_OLS = []; tau_prdctd_SDP = []; tau_prdctd_CAD = [];
for i = vldtnRange for i = vldtnRange
qi = [pendubot.shldr_position(i), pendubot.elbw_position(i)]'; qi = [pendubot.shldr_position(i), pendubot.elbw_position(i)]';
qdi = [pendubot.shldr_velocity_filtered(i), pendubot.elbw_velocity_filtered(i)]'; qdi = [pendubot.shldr_velocity_filtered(i), pendubot.elbw_velocity_filtered(i)]';
@ -63,14 +65,19 @@ for i = vldtnRange
Yfrctni = frictionRegressor(qdi); Yfrctni = frictionRegressor(qdi);
tau_prdctd_OLS = horzcat(tau_prdctd_OLS, [Ybi, Yfrctni]*pi_hat); tau_prdctd_OLS = horzcat(tau_prdctd_OLS, [Ybi, Yfrctni]*pi_hat);
tau_prdctd_SDP = horzcat(tau_prdctd_SDP, [Ybi, Yfrctni]*[pi_b; pi_frctn]); tau_prdctd_SDP = horzcat(tau_prdctd_SDP, [Ybi, Yfrctni]*[pi_b; pi_frctn]);
tau_prdctd_CAD = horzcat(tau_prdctd_CAD, Yi*pi_CAD);
end end
%% %%
figure figure
subplot(2,1,1)
plot(pendubot.time(vldtnRange), pendubot.torque(vldtnRange),'r') plot(pendubot.time(vldtnRange), pendubot.torque(vldtnRange),'r')
hold on hold on
plot(pendubot.time(vldtnRange), tau_prdctd_OLS(1,:),'b-') plot(pendubot.time(vldtnRange), tau_prdctd_OLS(1,:),'b-')
plot(pendubot.time(vldtnRange), tau_prdctd_SDP(1,:),'k-') plot(pendubot.time(vldtnRange), tau_prdctd_SDP(1,:),'k-')
% plot(pendubot.time(vldtnRange), pendubot.torque(vldtnRange) - tau_prdctd(1,:)', 'k--') plot(pendubot.time(vldtnRange), tau_prdctd_CAD(1,:),'g-')
legend('measured', 'predicted OLS', 'predicted SDP') legend('measured', 'predicted OLS', 'predicted SDP', 'predicted CAD')
grid on grid on
subplot(2,1,2)
plot(pendubot.time(vldtnRange), pendubot.shldr_acceleration_filtered(vldtnRange))

80
planar2DOF/pndbt_vldtn.m~ Normal file
View File

@ -0,0 +1,80 @@
% load pendubot data
rawData = load('position_A_0.7_v_1.0.mat');
% rawData = load('position_A_1.2_v_0.05.mat');
% parse pendubot data
pendubot.time = rawData.data(:,1) - rawData.data(1,1);
pendubot.current = rawData.data(:,2);
pendubot.current_desired = rawData.data(:,4);
pendubot.torque = rawData.data(:,3);
pendubot.shldr_position = rawData.data(:,7) - pi/2; % to fit model
pendubot.shldr_velocity = rawData.data(:,9);
pendubot.elbw_position = rawData.data(:,8);
pendubot.elbw_velocity = rawData.data(:,10);
pendubot.position_desired = rawData.data(:,5);
pendubot.velocity_desired = rawData.data(:,6);
% filter velocties with zero phas filter
vlcty_fltr = designfilt('lowpassiir','FilterOrder',5, ...
'HalfPowerFrequency',0.25,'DesignMethod','butter');
pendubot.shldr_velocity_filtered = filtfilt(vlcty_fltr, pendubot.shldr_velocity);
pendubot.elbw_velocity_filtered = filtfilt(vlcty_fltr, pendubot.elbw_velocity);
% estimating accelerations based on filtered velocities
q2d1 = zeros(size(pendubot.shldr_velocity));
q2d2 = zeros(size(pendubot.elbw_velocity));
for i = 2:size(pendubot.shldr_velocity,1)-1
q2d1(i) = (pendubot.shldr_velocity_filtered(i+1) - pendubot.shldr_velocity_filtered(i-1))/...
(pendubot.time(i+1) - pendubot.time(i-1));
q2d2(i) = (pendubot.elbw_velocity_filtered(i+1) - pendubot.elbw_velocity_filtered(i-1))/...
(pendubot.time(i+1) - pendubot.time(i-1));
end
pendubot.shldr_acceleration = q2d1;
pendubot.elbow_acceleration = q2d2;
% filter estimated accelerations with zero phase filter
aclrtn_fltr = designfilt('lowpassiir','FilterOrder',5, ...
'HalfPowerFrequency',0.1,'DesignMethod','butter');
pendubot.shldr_acceleration_filtered = filtfilt(aclrtn_fltr, pendubot.shldr_acceleration);
pendubot.elbow_acceleration_filtered = filtfilt(aclrtn_fltr, pendubot.elbow_acceleration);
% filter torque measurements using zero phase filter
trq_fltr = designfilt('lowpassiir','FilterOrder',5, ...
'HalfPowerFrequency',0.2,'DesignMethod','butter');
pendubot.torque_filtered = filtfilt(trq_fltr, pendubot.torque);
%%
% Validation
pi_CAD = [plnr.pi(:,1); 0; plnr.pi(:,2)];
vldtnRange = 1:1500;
tau_prdctd_OLS = []; tau_prdctd_SDP = []; tau_prdctd_CAD = [];
for i = vldtnRange
qi = [pendubot.shldr_position(i), pendubot.elbw_position(i)]';
qdi = [pendubot.shldr_velocity_filtered(i), pendubot.elbw_velocity_filtered(i)]';
q2di = [pendubot.shldr_acceleration_filtered(i), pendubot.elbow_acceleration_filtered(i)]';
if plnrBaseQR.motorDynamicsIncluded
Yi = regressorWithMotorDynamicsPndbt(qi, qdi, q2di);
else
Yi = full_regressor_plnr(qi, qdi, q2di);
end
Ybi = Yi*plnrBaseQR.permutationMatrix(:,1:plnrBaseQR.numberOfBaseParameters);
Yfrctni = frictionRegressor(qdi);
tau_prdctd_OLS = horzcat(tau_prdctd_OLS, [Ybi, Yfrctni]*pi_hat);
tau_prdctd_SDP = horzcat(tau_prdctd_SDP, [Ybi, Yfrctni]*[pi_b; pi_frctn]);
tau_prdctd_CAD = horzcat(tau_prdctd_CAD, Yi*pi_CAD);
end
%%
figure
plot(pendubot.time(vldtnRange), pendubot.torque(vldtnRange),'r')
hold on
plot(pendubot.time(vldtnRange), tau_prdctd_OLS(1,:),'b-')
plot(pendubot.time(vldtnRange), tau_prdctd_SDP(1,:),'k-')
plot(pendubot.time(vldtnRange), -tau_prdctd_CAD(1,:),'g-')
legend('measured', 'predicted OLS', 'predicted SDP', 'predicted CAD')
grid on

12
trajectory_optmzn/coeffs4_UR/a_coeffs.script
View File

@ -1,6 +1,6 @@
a1 = [63.14445447,3.8464372,0.4158832844,0.3695524202,0.08746106361,-0.1337522786,0.2961289419,0.1481741796,-0.2353883251,0.09399249647,-0.1538978199,0.233615134] a1 = [-21.87595086,-1.066284621,-0.4587618352,-0.286129784,-0.1574714979]
a2 = [64.11741816,3.419368669,0.9569679605,0.3840862501,0.07814075303,-0.1229639591,0.02985321809,0.1491091833,0.02899625285,0.05196032274,0.2596098438,0.07364049564] a2 = [2.254414645,0.5298068993,-0.2759654089,-0.08928062656,-0.05905944622]
a3 = [63.99585112,3.795840113,0.3484088895,0.4071788941,0.08260375437,0.2451950051,0.09884994438,-0.04640752829,-0.04826315454,0.2385501641,0.04460446056,0.1202201747] a3 = [2.14293594,0.06354360551,0.2829557411,-0.201319263,-0.03249596084]
a4 = [16.02097434,1.253074147,-0.255199746,0.4398665024,-0.2171605318,-0.0671930056,0.04760909641,0.05378361497,0.1664478999,-0.1412713881,-0.107594707,-0.01160368875] a4 = [-42.59095868,-2.859344916,-0.5487406736,-0.02351235854,-0.0944636514]
a5 = [8.719760589,0.812418026,-0.1530323563,0.1242863329,-0.09048595915,-0.07278439425,0.09679122168,-0.04824579569,-0.1194642801,0.08013340566,-0.01283551447,0.005705712498] a5 = [15.52365917,0.4769713622,0.4979022476,0.3917942812,-0.1159225628]
a6 = [64.61350353,3.897149875,0.8834439241,-0.007566380055,0.3211701597,0.1278674554,0.07810124808,0.1587321747,0.1003165104,0.03590044572,0.03013318791,-0.01484059122] a6 = [-5.565764374,-0.4629306537,-0.4980567899,0.3548393199,0.207181971]

12
trajectory_optmzn/coeffs4_UR/b_coeffs.script
View File

@ -1,6 +1,6 @@
b1 = [-0.7092040412,0.5230610431,0.1976180106,-0.2377492599,0.09662808559,-0.0442446606,0.04546091583,-0.05527008338,0.07999041891,-0.1954301078,0.02276381324,-0.0003037708343] b1 = [-10.06414131,-1.229285423,-0.2722715415,-0.6394408793,-0.08185129266]
b2 = [16.44144409,1.96646412,0.4413777885,0.01535646862,0.1699440808,-0.04517986656,0.08204914487,0.09802980971,0.1107566817,0.164257973,0.1687008776,-0.1158379341] b2 = [0.5282745708,0.1546858548,0.07585662935,-0.1232269499,0.04714922985]
b3 = [15.91827703,1.738289041,0.6396138544,0.4400174545,0.06841133754,0.1540512842,-0.01213731937,0.1288104914,0.01749713258,0.2106228112,-0.1025715439,-0.03232628298] b3 = [0.4948850456,-0.2634902153,0.3416064855,-0.3254315759,0.2266031683]
b4 = [3.07591063,0.4116519318,0.1342122544,-0.02000429905,0.1916582385,0.1154067377,0.03839347039,0.01232415683,-0.1919967103,-0.01691073997,0.01163710601,0.04178807472] b4 = [1.057259467,0.0996320244,0.4582898201,-0.02462075707,-0.1519593253]
b5 = [32.38341148,4.112282002,1.463403865,0.4409561611,0.3426812758,0.1595795442,0.1184208908,0.05993487595,-0.09285686628,0.01674492771,0.09477511091,0.168557236] b5 = [6.407866956,0.4273336966,0.2401572619,0.49581804,0.08918611026]
b6 = [32.04972222,3.852785925,1.284364462,-0.1618577222,0.456724459,0.2217872019,0.2525677551,0.1450619486,0.09508517903,0.04484496664,0.1612198891,-0.003833787606] b6 = [-1.485874308,-0.08921784544,-0.6665128301,0.1327547781,0.1467095463]

12
trajectory_optmzn/coeffs4_UR/c_coeffs.script
View File

@ -1,6 +1,6 @@
c1 = [-1.39519063326,-68.1126607632,0.14374629977,1.6884418891,-0.127351873182,0.00255422477862] c1 = [-34.8414733837,23.8445986014,2.56142637571,-0.852257602606,0.0511121883169,-0.000894172447552]
c2 = [54.6442907022,-69.4261871541,-4.21007966412,2.15666264527,-0.140699300074,0.00260348201828] c2 = [0.369385666851,-2.35991606261,-0.126928906857,0.0716907922511,-0.00474216488454,8.8496852348e-05]
c3 = [54.6708156721,-69.2826318354,-4.08927819065,2.14099361495,-0.140128130168,0.00259809869383] c3 = [1.40365589935,-2.25562006327,-0.129435755037,0.0693340770855,-0.00455287700623,8.45857523727e-05]
c4 = [9.14854552465,-17.1817325285,-0.781047135654,0.507648026779,-0.0341683663301,0.00064431496982] c4 = [2.32306208523,46.1170202779,-0.278519989178,-1.12507350803,0.0857731130481,-0.00172938826042]
c5 = [111.953260927,-9.34224698765,-8.34694011304,1.06825018599,-0.0383840633844,0.000350334262037] c5 = [21.7831485858,-16.7744044977,-1.63554594097,0.58291470654,-0.0355408732857,0.000629040168665]
c6 = [110.057813716,-70.2239115388,-8.2522415789,2.58082194636,-0.152300438083,0.00263339668271] c6 = [-5.37983041231,5.96473052648,0.376878279511,-0.186806091113,0.0121260654359,-0.000223677394743]

BIN
trajectory_optmzn/ga_N5T20.mat Normal file
View File

Binary file not shown.

BIN
trajectory_optmzn/ptrnSrch_N10T20QR.mat
View File

Binary file not shown.

59
trajectory_optmzn/writeTrajectoryCoefficientsIntoFile.m Normal file
View File

@ -0,0 +1,59 @@
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);

238
ur_idntfcn_real.m
View File

@ -3,114 +3,61 @@ clc; clear all; close all;
% ------------------------------------------------------------------------ % ------------------------------------------------------------------------
% Load data and procces it (filter and estimate accelerations) % Load data and procces it (filter and estimate accelerations)
% ------------------------------------------------------------------------ % ------------------------------------------------------------------------
unloadedTrajectory = parseURData('ur-19_12_23_free.csv', 1, 2005);
% unloadedTrajectory = parseURData('ur-20_01_31-unload.csv', 300, 2623);
unloadedTrajectory = filterData(unloadedTrajectory);
% ------------------------------------------------------------------------ % idntfcnTrjctry = parseURData('ur-19_12_23_free.csv', 1, 2005);
% idntfcnTrjctry = parseURData('ur-20_01_31-unload.csv', 300, 2623);
% idntfcnTrjctry = parseURData('ur-20_02_06-20sec_5harm.csv', 545, 2417); % 713
idntfcnTrjctry = parseURData('ur-20_02_05-20sec_8harm.csv', 320, 2310);
% idntfcnTrjctry = parseURData('ur-20_02_06-20sec_10harm.csv', 713, 2800); % 713
% idntfcnTrjctry = parseURData('ur-20_02_05-20sec_12harm.csv', 605, 2710);
idntfcnTrjctry = filterData(idntfcnTrjctry);
%{
idntfcnTrjctry1 = parseURData('ur-20_02_05-20sec_8harm.csv', 320, 2310);
idntfcnTrjctry2 = parseURData('ur-20_02_06-20sec_10harm.csv', 713, 2800);
idntfcnTrjctry1 = filterData(idntfcnTrjctry1);
idntfcnTrjctry2 = filterData(idntfcnTrjctry2);
idntfcnTrjctry3.t = [idntfcnTrjctry1.t; idntfcnTrjctry2.t + idntfcnTrjctry1.t(end)];
idntfcnTrjctry3.q = [idntfcnTrjctry1.q; idntfcnTrjctry2.q];
idntfcnTrjctry3.qd_fltrd = [idntfcnTrjctry1.qd_fltrd; idntfcnTrjctry2.qd_fltrd];
idntfcnTrjctry3.q2d_est = [idntfcnTrjctry1.q2d_est; idntfcnTrjctry2.q2d_est];
idntfcnTrjctry3.i_fltrd = [idntfcnTrjctry1.i_fltrd; idntfcnTrjctry2.i_fltrd];
%}
% -------------------------------------------------------------------
% Generate Regressors based on data % Generate Regressors based on data
% ------------------------------------------------------------------------ % ------------------------------------------------------------------------
% Load matrices that map standard set of paratmers to base parameters % Load matrices that map standard set of paratmers to base parameters
% load('full2base_mapping.mat'); % load('full2base_mapping.mat');
load('baseQR.mat'); % load mapping from full parameters to base parameters load('baseQR.mat'); % load mapping from full parameters to base parameters
E1 = baseQR.permutationMatrix(:,1:baseQR.numberOfBaseParameters);
% load identified drive gains % load identified drive gains
load('driveGains.mat') % load('driveGains.mat')
% drvGains = [14.87; 13.26; 11.13; 10.62; 11.03; 11.47]; % deLuca gains drvGains = [14.87; 13.26; 11.13; 10.62; 11.03; 11.47]; % deLuca gains
% drvGains = [11.1272; 11.83; 9.53; 12.64; 10.24; 5.53];
% drvGains = [15.87; 11.83; 9.53; 11.92; 15.24; 18.47];
%Constracting regressor matrix
Wb_uldd = []; Tau_uldd = [];
for i = 1:1:length(unloadedTrajectory.t)
Y_ulddi = regressorWithMotorDynamics(unloadedTrajectory.q(i,:)',...
unloadedTrajectory.qd_fltrd(i,:)',...
unloadedTrajectory.q2d_est(i,:)');
Yfrctni = frictionRegressor(unloadedTrajectory.qd_fltrd(i,:)');
Ybi_uldd = [Y_ulddi*E1, Yfrctni];
Wb_uldd = vertcat(Wb_uldd, Ybi_uldd);
Tau_uldd = vertcat(Tau_uldd, diag(drvGains)*unloadedTrajectory.i_fltrd(i,:)');
end
%% Usual least squares
pib_hat = (Wb_uldd'*Wb_uldd)\(Wb_uldd'*Tau_uldd);
pi_b = pib_hat(1:40); % variables for base paramters
pi_frctn = pib_hat(41:end);
Tau = {}; Wb = {};
[Tau{1}, Wb{1}] = buildObservationMatrices(idntfcnTrjctry, baseQR, drvGains);
[Tau{2}, Wb{2}] = buildObservationMatrices(idntfcnTrjctry, baseQR, drvGains2);
[Tau{3}, Wb{3}] = buildObservationMatrices(idntfcnTrjctry, baseQR, drvGains3);
%% Set-up SDP optimization procedure % Usual least squares
physicalConsistency = 1; [pib_OLS(:,1), pifrctn_OLS(:,1)] = ordinaryLeastSquareEstimation(Tau{1}, Wb{1});
[pib_OLS(:,2), pifrctn_OLS(:,2)] = ordinaryLeastSquareEstimation(Tau{2}, Wb{2});
[pib_OLS(:,3), pifrctn_OLS(:,3)] = ordinaryLeastSquareEstimation(Tau{3}, Wb{3});
pi_frctn = sdpvar(18,1); % Set-up SDP optimization procedure
pi_b = sdpvar(baseQR.numberOfBaseParameters,1); % variables for base paramters [pib_SDP(:,1), pifrctn_SDP(:,1)] = physicallyConsistentEstimation(Tau{1}, Wb{1}, baseQR);
pi_d = sdpvar(26,1); % variables for dependent paramters [pib_SDP(:,2), pifrctn_SDP(:,2)] = physicallyConsistentEstimation(Tau{2}, Wb{2}, baseQR);
[pib_SDP(:,3), pifrctn_SDP(:,3)] = physicallyConsistentEstimation(Tau{3}, Wb{3}, baseQR);
% Bijective mapping from [pi_b; pi_d] to standard parameters pi
pii = baseQR.permutationMatrix*[eye(baseQR.numberOfBaseParameters), ...
-baseQR.beta; ...
zeros(26,baseQR.numberOfBaseParameters), ...
eye(26) ]*[pi_b; pi_d];
% Feasibility contrraints of the link paramteres and rotor inertia
mass_indexes = 10:11:66;
massValuesURDF = [7.778 12.93 3.87 1.96 1.96 0.202]';
errorRange = 0.25;
massUpperBound = massValuesURDF*(1 + errorRange);
massLowerBound = massValuesURDF*(1 - errorRange);
cnstr = [];
for i = 1:6
cnstr = [cnstr, pii(mass_indexes(i))> massLowerBound(i), ...
pii(mass_indexes(i)) < massUpperBound(i)];
end
if physicalConsistency
for i = 1:11:66
link_inertia_i = [pii(i), pii(i+1), pii(i+2); ...
pii(i+1), pii(i+3), pii(i+4); ...
pii(i+2), pii(i+4), pii(i+5)];
frst_mmnt_i = pii(i+6:i+8);
Di = [0.5*trace(link_inertia_i)*eye(3) - link_inertia_i, ...
frst_mmnt_i; frst_mmnt_i', pii(i+9)];
cnstr = [cnstr, Di>0, pii(i+10)>0, pii(i+9)<15];
end
else
for i = 1:11:66
link_inertia_i = [pii(i), pii(i+1), pii(i+2); ...
pii(i+1), pii(i+3), pii(i+4); ...
pii(i+2), pii(i+4), pii(i+5)];
frst_mmnt_i = vec2skewSymMat(pii(i+6:i+8));
Di = [link_inertia_i, frst_mmnt_i'; frst_mmnt_i, pii(i+9)*eye(3)];
cnstr = [cnstr, Di>0, pii(i+10)>0];
end
end
% Feasibility constraints on the friction prameters
for i = 1:6
cnstr = [cnstr, pi_frctn(3*i-2)>0, pi_frctn(3*i-1)>0];
end
% Defining pbjective function
obj = norm(Tau_uldd - Wb_uldd*[pi_b; pi_frctn]);
% Solving sdp problem
sol2 = optimize(cnstr, obj, sdpsettings('solver','sdpt3'));
pi_frctn = value(pi_frctn);
pi_b = value(pi_b); % variables for base paramters
return
%% Saving identified parameters %% Saving identified parameters
pi_full = baseQR.permutationMatrix*[eye(baseQR.numberOfBaseParameters), ... pi_full = baseQR.permutationMatrix*[eye(baseQR.numberOfBaseParameters), ...
@ -138,3 +85,106 @@ sgma_pi = sqrt(diag(Cpi));
% relative standard deviation % relative standard deviation
sgma_pi_rltv = sgma_pi./abs([pi_b; pi_frctn]); sgma_pi_rltv = sgma_pi./abs([pi_b; pi_frctn]);
%% Functions
function [Tau, Wb] = buildObservationMatrices(idntfcnTrjctry, baseQR, drvGains)
% --------------------------------------------------------------------
% The function builds observation matrix for UR10E
% --------------------------------------------------------------------
E1 = baseQR.permutationMatrix(:,1:baseQR.numberOfBaseParameters);
Wb = []; Tau = [];
for i = 1:1:length(idntfcnTrjctry.t)
Yi = regressorWithMotorDynamics(idntfcnTrjctry.q(i,:)',...
idntfcnTrjctry.qd_fltrd(i,:)',...
idntfcnTrjctry.q2d_est(i,:)');
Yfrctni = frictionRegressor(idntfcnTrjctry.qd_fltrd(i,:)');
Ybi = [Yi*E1, Yfrctni];
Wb = vertcat(Wb, Ybi);
Tau = vertcat(Tau, diag(drvGains)*idntfcnTrjctry.i_fltrd(i,:)');
end
end
function [pib_OLS, pifrctn_OLS] = ordinaryLeastSquareEstimation(Tau, Wb)
pi_OLS = (Wb'*Wb)\(Wb'*Tau);
pib_OLS = pi_OLS(1:40); % variables for base paramters
pifrctn_OLS = pi_OLS(41:end);
end
function [pib_SDP, pifrctn_SDP] = physicallyConsistentEstimation(Tau, Wb, baseQR)
physicalConsistency = 1;
pi_frctn = sdpvar(18,1); % variables for dependent parameters
pi_b = sdpvar(baseQR.numberOfBaseParameters,1); % variables for base paramters
pi_d = sdpvar(26,1); % variables for dependent paramters
% Bijective mapping from [pi_b; pi_d] to standard parameters pi
pii = baseQR.permutationMatrix*[eye(baseQR.numberOfBaseParameters), ...
-baseQR.beta; ...
zeros(26,baseQR.numberOfBaseParameters), ...
eye(26) ]*[pi_b; pi_d];
% Feasibility contrraints of the link paramteres and rotor inertia
mass_indexes = 10:11:66;
massValuesURDF = [7.778 12.93 3.87 1.96 1.96 0.202]';
errorRange = 0.10;
massUpperBound = massValuesURDF*(1 + errorRange);
cnstr = [];
for i = 1:6
cnstr = [cnstr, pii(mass_indexes(i))> 0, ...
pii(mass_indexes(i)) < massUpperBound(i)];
end
if physicalConsistency
for i = 1:11:66
link_inertia_i = [pii(i), pii(i+1), pii(i+2); ...
pii(i+1), pii(i+3), pii(i+4); ...
pii(i+2), pii(i+4), pii(i+5)];
frst_mmnt_i = pii(i+6:i+8);
Di = [0.5*trace(link_inertia_i)*eye(3) - link_inertia_i, ...
frst_mmnt_i; frst_mmnt_i', pii(i+9)];
cnstr = [cnstr, Di>0, pii(i+10)>0];
end
else
for i = 1:11:66
link_inertia_i = [pii(i), pii(i+1), pii(i+2); ...
pii(i+1), pii(i+3), pii(i+4); ...
pii(i+2), pii(i+4), pii(i+5)];
frst_mmnt_i = vec2skewSymMat(pii(i+6:i+8));
Di = [link_inertia_i, frst_mmnt_i'; frst_mmnt_i, pii(i+9)*eye(3)];
cnstr = [cnstr, Di>0, pii(i+10)>0];
end
end
% Feasibility constraints on the friction prameters
for i = 1:6
cnstr = [cnstr, pi_frctn(3*i-2)>0, pi_frctn(3*i-1)>0];
end
% Defining pbjective function
obj = norm(Tau - Wb*[pi_b; pi_frctn]);
% Solving sdp problem
sol2 = optimize(cnstr, obj, sdpsettings('solver','sdpt3'));
pib_SDP = value(pi_b); % variables for base paramters
pifrctn_SDP = value(pi_frctn);
end

54
ur_knmtcs.m
View File

@ -32,7 +32,8 @@ matlabFunction(pos,'File','autogen/position_fk_UR10E','Vars',{q_sym})
% load('ptrnSrch_N6T20QR.mat'); % load('ptrnSrch_N6T20QR.mat');
% load('ptrnSrch_N8T20QR.mat'); % load('ptrnSrch_N8T20QR.mat');
% load('ptrnSrch_N10T20QR.mat'); % load('ptrnSrch_N10T20QR.mat');
load('ptrnSrch_N12T20QR.mat'); % load('ptrnSrch_N12T20QR.mat');
load('ga_N5T20.mat');
traj_par.t_smp = 5e-2; traj_par.t_smp = 5e-2;
traj_par.t = 0:traj_par.t_smp:traj_par.T; traj_par.t = 0:traj_par.t_smp:traj_par.T;
[q,qd,q2d] = mixed_traj(traj_par.t, c_pol, a, b, traj_par.wf, traj_par.N); [q,qd,q2d] = mixed_traj(traj_par.t, c_pol, a, b, traj_par.wf, traj_par.N);
@ -45,7 +46,7 @@ for i = 1:size(q,2)
end end
% Needed for surface % Needed for surface
[x, y] = meshgrid(-0.7:0.05:0.7); % Generate x and y data [x, y] = meshgrid(-0.4:0.05:0.4); % Generate x and y data
z = zeros(size(x, 1)); % Generate z data z = zeros(size(x, 1)); % Generate z data
figure figure
@ -78,53 +79,4 @@ writeTrajectoryCoefficientsIntoFile(a, b, c_pol)
% axis equal % axis equal
function writeTrajectoryCoefficientsIntoFile(a, b, c_pol)
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);
end

123
ur_vldtn.m
View File

@ -2,11 +2,12 @@
% Load validation trajectory % Load validation trajectory
% ------------------------------------------------------------------------ % ------------------------------------------------------------------------
% vldtnTrjctry = parseURData('ur-19_10_01-14_04_13.csv', 1, 759); vldtnTrjctry = parseURData('ur-19_10_01-14_04_13.csv', 1, 759);
% vldtnTrjctry = parseURData('ur-19_10_01-13_51_41.csv', 1, 660); % vldtnTrjctry = parseURData('ur-19_10_01-13_51_41.csv', 1, 660);
% vldtnTrjctry = parseURData('ur-19_09_27-11_28_22.csv', 1, 594); % vldtnTrjctry = parseURData('ur-19_09_27-11_28_22.csv', 1, 594);
vldtnTrjctry = parseURData('ur-20_01_17-p2.csv', 1, 250); % vldtnTrjctry = parseURData('ur-20_01_17-p8.csv', 1, 250);
% vldtnTrjctry = parseURData('ur-20_01_17-ptp_10_points.csv', 1, 5346); % vldtnTrjctry = parseURData('ur-20_01_17-ptp_10_points.csv', 1, 5346);
% vldtnTrjctry = parseURData('ur-19_12_23_free.csv', 1, 2005);
vldtnTrjctry = filterData(vldtnTrjctry); vldtnTrjctry = filterData(vldtnTrjctry);
@ -14,121 +15,49 @@ vldtnTrjctry = filterData(vldtnTrjctry);
% Predicting torques % Predicting torques
% ----------------------------------------------------------------------- % -----------------------------------------------------------------------
%Constracting regressor matrix %Constracting regressor matrix
tau_msrd = []; tau_prdctd1 = []; tau_prdctd2 = []; tau_msrd = []; tau_OLS = []; tau_SDP = [];
i_prdct1 = []; i_prdct2 = []; i_OLS1 = []; i_OLS2 = []; i_OLS3 = [];
i_SPD1 = []; i_SPD2 = []; i_SPD3 = [];
for i = 1:length(vldtnTrjctry.t) for i = 1:length(vldtnTrjctry.t)
Yi = regressorWithMotorDynamics(vldtnTrjctry.q(i,:)',... Yi = regressorWithMotorDynamics(vldtnTrjctry.q(i,:)',...
vldtnTrjctry.qd_fltrd(i,:)',... vldtnTrjctry.qd_fltrd(i,:)',...
vldtnTrjctry.q2d_est(i,:)'); vldtnTrjctry.q2d_est(i,:)');
tau_withoutFriction = Yi*E1*pi_b; Ybi = Yi*baseQR.permutationMatrix(:,1:baseQR.numberOfBaseParameters);
Yfrctni = frictionRegressor(vldtnTrjctry.qd_fltrd(i,:)');
tau_lnr_frcn = zeros(6,1); % tau_msrd = horzcat(tau_msrd, diag(drvGains)*vldtnTrjctry.i(i,:)');
% tau_nonlnr_frcn = zeros(6,1); % tau_OLS = horzcat(tau_OLS, [Ybi Yfrctni]*[pib_OLS; pifrctn_OLS]);
for j = 1:6 % tau_SDP = horzcat(tau_SDP, [Ybi Yfrctni]*[pib_SDP; pifrctn_SDP]);
tau_lnr_frcn(j) = linearFrictionModel(pi_frctn(3*(j-1)+1:3*(j-1)+3),...
vldtnTrjctry.qd_fltrd(i,j)');
% tau_nonlnr_frcn(j) = nonlinearFrictionModel(pi_nonlnr_frcn(5*(j-1)+1:5*(j-1)+5),...
% vldtnTrjctry.qd_fltrd(i,j)');
end
tau_msrd = horzcat(tau_msrd, diag(drvGains)*vldtnTrjctry.i(i,:)');
tau_prdctd1 = horzcat(tau_prdctd1, tau_withoutFriction + tau_lnr_frcn);
% tau_prdctd2 = horzcat(tau_prdctd2, tau_withoutFriction + tau_nonlnr_frcn);
i_prdct1 = horzcat(i_prdct1, diag(drvGains)\(tau_withoutFriction + tau_lnr_frcn)); i_OLS1 = horzcat(i_OLS1, diag(drvGains)\([Ybi Yfrctni]*[pib_OLS(:,1); pifrctn_OLS(:,1)]));
% i_prdct2 = horzcat(i_prdct2, diag(drvGains)\(tau_withoutFriction + tau_nonlnr_frcn)); i_OLS2 = horzcat(i_OLS2, diag(drvGains2)\([Ybi Yfrctni]*[pib_OLS(:,2); pifrctn_OLS(:,2)]));
i_OLS3 = horzcat(i_OLS3, diag(drvGains3)\([Ybi Yfrctni]*[pib_OLS(:,3); pifrctn_OLS(:,3)]));
i_SPD1 = horzcat(i_SPD1, diag(drvGains)\([Ybi Yfrctni]*[pib_SDP(:,1); pifrctn_SDP(:,1)]));
i_SPD2 = horzcat(i_SPD2, diag(drvGains2)\([Ybi Yfrctni]*[pib_SDP(:,2); pifrctn_SDP(:,2)]));
i_SPD3 = horzcat(i_SPD3, diag(drvGains3)\([Ybi Yfrctni]*[pib_SDP(:,3); pifrctn_SDP(:,3)]));
end end
%% %%
plotTorquesWithLinearFriction = 0;
plotTorquesWithNonlinearFriction = 0;
for i = 1:6 for i = 1:6
figure figure
hold on hold on
plot(vldtnTrjctry.t, vldtnTrjctry.i(:,i), 'r-') plot(vldtnTrjctry.t, vldtnTrjctry.i(:,i), 'r-')
plot(vldtnTrjctry.t, i_prdct1(i,:), 'k-') plot(vldtnTrjctry.t, i_OLS1(i,:), 'k-', 'LineWidth',1.5)
legend('measured', 'prdctd1') plot(vldtnTrjctry.t, i_OLS2(i,:), 'k--', 'LineWidth',1.5)
plot(vldtnTrjctry.t, i_OLS3(i,:), 'k-.', 'LineWidth',1.5)
plot(vldtnTrjctry.t, i_SPD1(i,:), 'b-', 'LineWidth',1.5)
plot(vldtnTrjctry.t, i_SPD2(i,:), 'b--', 'LineWidth',1.5)
plot(vldtnTrjctry.t, i_SPD3(i,:), 'b-.', 'LineWidth',1.5)
legend('measured', 'OLS1', 'OLS2', 'OLS3', 'SDP1', 'SDP2', 'SDP3')
grid on grid on
end end
if plotTorquesWithLinearFriction
for i = 1:6
figure
hold on
plot(vldtnTrjctry.t, tau_msrd(i,:), 'r-')
plot(vldtnTrjctry.t, tau_prdctd1(i,:), 'k-')
plot(vldtnTrjctry.t, tau_prdctd1(i,:) - tau_msrd(i,:), '--')
legend('measured', 'predicted', 'error')
grid on
end
end
if plotTorquesWithNonlinearFriction
for i = 1:6
figure
hold on
plot(vldtnTrjctry.t, tau_msrd(i,:), 'r-')
plot(vldtnTrjctry.t, tau_prdctd2(i,:), 'k-')
plot(vldtnTrjctry.t, tau_prdctd2(i,:) - tau_msrd(i,:), '--')
legend('measured', 'predicted', 'error')
grid on
end
end
return
for i = 1:6
figure
hold on
plot(vldtnTrjctry.t, vldtnTrjctry.i(:,i),'r-')
plot(vldtnTrjctry.t, i_prdct1(i,:),'k-')
plot(vldtnTrjctry.t, i_prdct2(i,:),'b-')
legend('real','with linear friction', 'with nonlinear friction')
grid on
end
return
fig = figure;
subplot(3,2,1)
plot(t_msrd,tau_msrd(1,:),'r-')
hold on
plot(t_msrd,tau_prdctd(1,:),'k-')
ylabel('\tau_1')
grid on
subplot(3,2,2)
plot(t_msrd,tau_msrd(2,:),'r-')
hold on
plot(t_msrd,tau_prdctd(2,:),'k-')
ylabel('\tau_2')
grid on
subplot(3,2,3)
plot(t_msrd,tau_msrd(3,:),'r-')
hold on
plot(t_msrd,tau_prdctd(3,:),'k-')
ylabel('\tau_3')
grid on
subplot(3,2,4)
plot(t_msrd,tau_msrd(4,:),'r-')
hold on
plot(t_msrd,tau_prdctd(4,:),'k-')
ylabel('\tau_4')
grid on
subplot(3,2,5)
plot(t_msrd,tau_msrd(5,:),'r-')
hold on
plot(t_msrd,tau_prdctd(5,:),'k-')
ylabel('\tau_5')
xlabel('t')
grid on
subplot(3,2,6)
plot(t_msrd,tau_msrd(6,:),'r-')
hold on
plot(t_msrd,tau_prdctd(6,:),'k-')
ylabel('\tau_6')
xlabel('t')
grid on