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
parent
fe3cc01102
commit
40df589ea7
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
|
|
@ -12,12 +12,14 @@ function data = filterData(data)
|
|||
% filtered currents, data.q2d_est - estimated accelerations
|
||||
% ---------------------------------------------------------------------
|
||||
|
||||
% data = parseURData('ur-20_02_05-20sec_8harm.csv', 320, 2310);
|
||||
|
||||
% ---------------------------------------------------------------------
|
||||
% Filtering Velocities
|
||||
% ---------------------------------------------------------------------
|
||||
% Design filter
|
||||
vel_filt = designfilt('lowpassiir','FilterOrder',3, ...
|
||||
'HalfPowerFrequency',0.2,'DesignMethod','butter');
|
||||
vel_filt = designfilt('lowpassiir','FilterOrder',5, ...
|
||||
'HalfPowerFrequency',0.15,'DesignMethod','butter');
|
||||
|
||||
data.qd_fltrd = zeros(size(data.qd));
|
||||
for i = 1:6
|
||||
|
|
@ -38,7 +40,7 @@ end
|
|||
|
||||
% Zeros phase filtering acceleration obtained by finite difference
|
||||
accel_filt = designfilt('lowpassiir','FilterOrder',5, ...
|
||||
'HalfPowerFrequency',0.05,'DesignMethod','butter');
|
||||
'HalfPowerFrequency',0.15,'DesignMethod','butter');
|
||||
for i = 1:6
|
||||
data.q2d_est(:,i) = filtfilt(accel_filt,data.q2d_est(:,i));
|
||||
end
|
||||
|
|
@ -48,7 +50,7 @@ end
|
|||
% ------------------------------------------------------------------------
|
||||
% Zeros phase filtering acceleration obtained by finite difference
|
||||
curr_filt = designfilt('lowpassiir','FilterOrder',5, ...
|
||||
'HalfPowerFrequency',0.1,'DesignMethod','butter');
|
||||
'HalfPowerFrequency',0.20,'DesignMethod','butter');
|
||||
|
||||
for i = 1:6
|
||||
data.i_fltrd(:,i) = filtfilt(curr_filt,data.i(:,i));
|
||||
|
|
@ -64,3 +66,27 @@ end
|
|||
for i = 1:6
|
||||
data.tau_des_fltrd(:,i) = filtfilt(curr_filt,data.tau_des(:,i));
|
||||
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
|
||||
|
|
@ -1,6 +1,7 @@
|
|||
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);
|
||||
end_time = 20;
|
||||
|
||||
|
|
|
|||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -3,7 +3,7 @@ function 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.
|
||||
% 03-Feb-2020 10:03:33
|
||||
% 07-Feb-2020 10:59:41
|
||||
|
||||
q1 = in1(1,:);
|
||||
q2 = in1(2,:);
|
||||
|
|
@ -17,8 +17,7 @@ t4 = sin(q2);
|
|||
t5 = cos(q1);
|
||||
t6 = sin(q1);
|
||||
t7 = qd2.^2;
|
||||
t8 = t2.*t5.*(9.81e2./1.0e2);
|
||||
t9 = q2d1+q2d2;
|
||||
t10 = t2.*t3.*t5.*(9.81e2./1.0e2);
|
||||
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,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]);
|
||||
t8 = q2d1+q2d2;
|
||||
t9 = t2.*t3.*t5.*(9.81e2./1.0e2);
|
||||
t10 = 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]);
|
||||
|
|
|
|||
|
|
@ -14,10 +14,10 @@
|
|||
|
||||
<link name = "link 1">
|
||||
<inertial>
|
||||
<origin xyz = "0.125 0 0"
|
||||
<origin xyz = "0.043 0 0"
|
||||
rpy = "0 0 0"/>
|
||||
<mass value = "0.29" />
|
||||
<inertia ixx = "1" iyy = "1" izz = "0.5"
|
||||
<mass value = "1.085" />
|
||||
<inertia ixx = "1" iyy = "1" izz = "0.008"
|
||||
ixy = "0.01" ixz = "0.01" iyz = "0.01" />
|
||||
</inertial>
|
||||
<visual>
|
||||
|
|
@ -34,10 +34,10 @@
|
|||
|
||||
<link name = "link 2">
|
||||
<inertial>
|
||||
<origin xyz = "0.125 0 0"
|
||||
<origin xyz = "0.0954 0 0"
|
||||
rpy = "0 0 0"/>
|
||||
<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" />
|
||||
</inertial>
|
||||
<visual>
|
||||
|
|
|
|||
Binary file not shown.
|
|
@ -9,8 +9,8 @@ includeMotorDynamics = 1;
|
|||
|
||||
|
||||
% limits on positions, velocities, accelerations
|
||||
q_min = -deg2rad(160);
|
||||
q_max = 0;
|
||||
q_min = -2*pi;
|
||||
q_max = 2*pi;
|
||||
qd_min = -10;
|
||||
qd_max = 10;
|
||||
q2d_min = -100;
|
||||
|
|
@ -97,7 +97,6 @@ pi_lgr_base = pi1 + beta*pi2;
|
|||
pi_lgr_base2 = [eye(bb) beta]*[pi1;pi2];
|
||||
pi_lgr_base3 = [eye(bb) beta]*E'*pi_pndbt_sym;
|
||||
|
||||
|
||||
% -----------------------------------------------------------------------
|
||||
% Validation of obtained mappings
|
||||
% -----------------------------------------------------------------------
|
||||
|
|
|
|||
|
|
@ -32,7 +32,7 @@ for i = 1:2
|
|||
plnr.h(:,i) = link_mass*com_pos;
|
||||
plnr.I_vec(:,i) = inertiaMatrix2Vector(link_inertia-...
|
||||
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
|
||||
|
||||
return
|
||||
|
|
|
|||
|
|
@ -71,11 +71,12 @@ for i = 1:noIter
|
|||
Ylgr = Y_fcn(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
|
||||
err1(i) = norm(tau1 - tau2);
|
||||
end
|
||||
plot(err1)
|
||||
|
||||
if all(err1<1e-6)
|
||||
disp('Regressor matrix is computed correctly!')
|
||||
|
|
|
|||
|
|
@ -3,8 +3,8 @@
|
|||
run('plnr_idntfcn.m')
|
||||
|
||||
% load pendubot data
|
||||
% rawData = load('current_A_5_v_4.mat');
|
||||
rawData = load('current_A_0.7_v_1.mat');
|
||||
% rawData = load('position_A_0.7_v_0.5.mat');
|
||||
rawData = load('position_A_1.2_v_0.05.mat');
|
||||
|
||||
% parse pendubot data
|
||||
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_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');
|
||||
|
|
@ -50,6 +47,12 @@ trq_fltr = designfilt('lowpassiir','FilterOrder',5, ...
|
|||
pendubot.torque_filtered = filtfilt(trq_fltr, pendubot.torque);
|
||||
|
||||
%%
|
||||
|
||||
% figure
|
||||
% plot(pendubot.time, pendubot.current*0.123)
|
||||
% hold on
|
||||
% plot(pendubot.time, pendubot.torque)
|
||||
|
||||
return
|
||||
plotJointPositions(pendubot)
|
||||
plotJointVelocities(pendubot)
|
||||
|
|
@ -64,10 +67,13 @@ function plotJointPositions(pendubot)
|
|||
figure
|
||||
subplot(2,1,1)
|
||||
plot(pendubot.time, pendubot.shldr_position)
|
||||
hold on
|
||||
plot(pendubot.time, pendubot.current_desired)
|
||||
xlim([0 1])
|
||||
xlabel('$t$, sec','interpreter', 'latex')
|
||||
ylabel('$q_1$, rad','interpreter', 'latex')
|
||||
grid on
|
||||
legend('measured', 'desired')
|
||||
subplot(2,1,2)
|
||||
plot(pendubot.time, pendubot.elbw_position)
|
||||
xlim([0 1])
|
||||
|
|
|
|||
|
|
@ -7,7 +7,7 @@ load('plnrBaseQR.mat')
|
|||
% compose observation matrix and torque vector
|
||||
noObservations = length(pendubot.time);
|
||||
W = []; Tau = [];
|
||||
for i = 1:noObservations
|
||||
for i = 1:1:noObservations
|
||||
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)]';
|
||||
|
|
@ -21,7 +21,8 @@ for i = 1:noObservations
|
|||
Yfrctni = frictionRegressor(qdi);
|
||||
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);
|
||||
end
|
||||
|
||||
|
|
@ -31,7 +32,7 @@ pi_hat = (W'*W)\(W'*Tau)
|
|||
|
||||
|
||||
%% Set-up SDP optimization procedure
|
||||
physicalConsistency = 1;
|
||||
physicalConsistency = 0;
|
||||
|
||||
pi_frctn = sdpvar(6,1);
|
||||
pi_b = sdpvar(plnrBaseQR.numberOfBaseParameters,1); % variables for base paramters
|
||||
|
|
@ -70,10 +71,10 @@ else
|
|||
end
|
||||
cnstr = [cnstr, pii(11) > 0]; % first motor inertia constraint
|
||||
|
||||
% Feasibility constraints on the friction prameters
|
||||
for i = 1:2
|
||||
cnstr = [cnstr, pi_frctn(3*i-2)>0, pi_frctn(3*i-1)>0];
|
||||
end
|
||||
% % Feasibility constraints on the friction prameters
|
||||
% for i = 1:2
|
||||
% cnstr = [cnstr, pi_frctn(3*i-2)>0, pi_frctn(3*i-1)>0];
|
||||
% end
|
||||
|
||||
% Defining pbjective function
|
||||
obj = norm(Tau - W*[pi_b; pi_frctn], 2);
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
% load pendubot data
|
||||
% rawData = load('current_A_0.7_v_1.mat');
|
||||
rawData = load('current_A_5_v_4.mat');
|
||||
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);
|
||||
|
|
@ -36,7 +36,7 @@ pendubot.elbow_acceleration = q2d2;
|
|||
|
||||
% filter estimated accelerations with zero phase filter
|
||||
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.elbow_acceleration_filtered = filtfilt(aclrtn_fltr, pendubot.elbow_acceleration);
|
||||
|
||||
|
|
@ -45,10 +45,12 @@ trq_fltr = designfilt('lowpassiir','FilterOrder',5, ...
|
|||
'HalfPowerFrequency',0.2,'DesignMethod','butter');
|
||||
pendubot.torque_filtered = filtfilt(trq_fltr, pendubot.torque);
|
||||
|
||||
%%
|
||||
|
||||
% Validation
|
||||
vldtnRange = 1:200;
|
||||
tau_prdctd_OLS = []; tau_prdctd_SDP = [];
|
||||
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)]';
|
||||
|
|
@ -63,14 +65,19 @@ for i = vldtnRange
|
|||
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
|
||||
subplot(2,1,1)
|
||||
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), pendubot.torque(vldtnRange) - tau_prdctd(1,:)', 'k--')
|
||||
legend('measured', 'predicted OLS', 'predicted SDP')
|
||||
plot(pendubot.time(vldtnRange), tau_prdctd_CAD(1,:),'g-')
|
||||
legend('measured', 'predicted OLS', 'predicted SDP', 'predicted CAD')
|
||||
grid on
|
||||
subplot(2,1,2)
|
||||
plot(pendubot.time(vldtnRange), pendubot.shldr_acceleration_filtered(vldtnRange))
|
||||
|
|
@ -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
|
||||
|
|
@ -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]
|
||||
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]
|
||||
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]
|
||||
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]
|
||||
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]
|
||||
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]
|
||||
a1 = [-21.87595086,-1.066284621,-0.4587618352,-0.286129784,-0.1574714979]
|
||||
a2 = [2.254414645,0.5298068993,-0.2759654089,-0.08928062656,-0.05905944622]
|
||||
a3 = [2.14293594,0.06354360551,0.2829557411,-0.201319263,-0.03249596084]
|
||||
a4 = [-42.59095868,-2.859344916,-0.5487406736,-0.02351235854,-0.0944636514]
|
||||
a5 = [15.52365917,0.4769713622,0.4979022476,0.3917942812,-0.1159225628]
|
||||
a6 = [-5.565764374,-0.4629306537,-0.4980567899,0.3548393199,0.207181971]
|
||||
|
|
|
|||
|
|
@ -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]
|
||||
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]
|
||||
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]
|
||||
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]
|
||||
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]
|
||||
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]
|
||||
b1 = [-10.06414131,-1.229285423,-0.2722715415,-0.6394408793,-0.08185129266]
|
||||
b2 = [0.5282745708,0.1546858548,0.07585662935,-0.1232269499,0.04714922985]
|
||||
b3 = [0.4948850456,-0.2634902153,0.3416064855,-0.3254315759,0.2266031683]
|
||||
b4 = [1.057259467,0.0996320244,0.4582898201,-0.02462075707,-0.1519593253]
|
||||
b5 = [6.407866956,0.4273336966,0.2401572619,0.49581804,0.08918611026]
|
||||
b6 = [-1.485874308,-0.08921784544,-0.6665128301,0.1327547781,0.1467095463]
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
c1 = [-1.39519063326,-68.1126607632,0.14374629977,1.6884418891,-0.127351873182,0.00255422477862]
|
||||
c2 = [54.6442907022,-69.4261871541,-4.21007966412,2.15666264527,-0.140699300074,0.00260348201828]
|
||||
c3 = [54.6708156721,-69.2826318354,-4.08927819065,2.14099361495,-0.140128130168,0.00259809869383]
|
||||
c4 = [9.14854552465,-17.1817325285,-0.781047135654,0.507648026779,-0.0341683663301,0.00064431496982]
|
||||
c5 = [111.953260927,-9.34224698765,-8.34694011304,1.06825018599,-0.0383840633844,0.000350334262037]
|
||||
c6 = [110.057813716,-70.2239115388,-8.2522415789,2.58082194636,-0.152300438083,0.00263339668271]
|
||||
c1 = [-34.8414733837,23.8445986014,2.56142637571,-0.852257602606,0.0511121883169,-0.000894172447552]
|
||||
c2 = [0.369385666851,-2.35991606261,-0.126928906857,0.0716907922511,-0.00474216488454,8.8496852348e-05]
|
||||
c3 = [1.40365589935,-2.25562006327,-0.129435755037,0.0693340770855,-0.00455287700623,8.45857523727e-05]
|
||||
c4 = [2.32306208523,46.1170202779,-0.278519989178,-1.12507350803,0.0857731130481,-0.00172938826042]
|
||||
c5 = [21.7831485858,-16.7744044977,-1.63554594097,0.58291470654,-0.0355408732857,0.000629040168665]
|
||||
c6 = [-5.37983041231,5.96473052648,0.376878279511,-0.186806091113,0.0121260654359,-0.000223677394743]
|
||||
|
|
|
|||
Binary file not shown.
Binary file not shown.
|
|
@ -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);
|
||||
|
||||
|
|
@ -3,114 +3,61 @@ clc; clear all; close all;
|
|||
% ------------------------------------------------------------------------
|
||||
% 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
|
||||
% ------------------------------------------------------------------------
|
||||
% Load matrices that map standard set of paratmers to base parameters
|
||||
% load('full2base_mapping.mat');
|
||||
load('baseQR.mat'); % load mapping from full parameters to base parameters
|
||||
E1 = baseQR.permutationMatrix(:,1:baseQR.numberOfBaseParameters);
|
||||
|
||||
|
||||
% load identified drive gains
|
||||
load('driveGains.mat')
|
||||
% 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);
|
||||
% load('driveGains.mat')
|
||||
drvGains = [14.87; 13.26; 11.13; 10.62; 11.03; 11.47]; % deLuca gains
|
||||
|
||||
|
||||
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
|
||||
physicalConsistency = 1;
|
||||
% Usual least squares
|
||||
[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);
|
||||
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.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
|
||||
% Set-up SDP optimization procedure
|
||||
[pib_SDP(:,1), pifrctn_SDP(:,1)] = physicallyConsistentEstimation(Tau{1}, Wb{1}, baseQR);
|
||||
[pib_SDP(:,2), pifrctn_SDP(:,2)] = physicallyConsistentEstimation(Tau{2}, Wb{2}, baseQR);
|
||||
[pib_SDP(:,3), pifrctn_SDP(:,3)] = physicallyConsistentEstimation(Tau{3}, Wb{3}, baseQR);
|
||||
|
||||
return
|
||||
|
||||
%% Saving identified parameters
|
||||
pi_full = baseQR.permutationMatrix*[eye(baseQR.numberOfBaseParameters), ...
|
||||
|
|
@ -138,3 +85,106 @@ sgma_pi = sqrt(diag(Cpi));
|
|||
% relative standard deviation
|
||||
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
54
ur_knmtcs.m
|
|
@ -32,7 +32,8 @@ matlabFunction(pos,'File','autogen/position_fk_UR10E','Vars',{q_sym})
|
|||
% load('ptrnSrch_N6T20QR.mat');
|
||||
% load('ptrnSrch_N8T20QR.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 = 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);
|
||||
|
|
@ -45,7 +46,7 @@ for i = 1:size(q,2)
|
|||
end
|
||||
|
||||
% 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
|
||||
|
||||
figure
|
||||
|
|
@ -78,53 +79,4 @@ writeTrajectoryCoefficientsIntoFile(a, b, c_pol)
|
|||
% 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
123
ur_vldtn.m
|
|
@ -2,11 +2,12 @@
|
|||
% 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_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-19_12_23_free.csv', 1, 2005);
|
||||
vldtnTrjctry = filterData(vldtnTrjctry);
|
||||
|
||||
|
||||
|
|
@ -14,121 +15,49 @@ vldtnTrjctry = filterData(vldtnTrjctry);
|
|||
% Predicting torques
|
||||
% -----------------------------------------------------------------------
|
||||
%Constracting regressor matrix
|
||||
tau_msrd = []; tau_prdctd1 = []; tau_prdctd2 = [];
|
||||
i_prdct1 = []; i_prdct2 = [];
|
||||
tau_msrd = []; tau_OLS = []; tau_SDP = [];
|
||||
i_OLS1 = []; i_OLS2 = []; i_OLS3 = [];
|
||||
i_SPD1 = []; i_SPD2 = []; i_SPD3 = [];
|
||||
for i = 1:length(vldtnTrjctry.t)
|
||||
Yi = regressorWithMotorDynamics(vldtnTrjctry.q(i,:)',...
|
||||
vldtnTrjctry.qd_fltrd(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_nonlnr_frcn = zeros(6,1);
|
||||
for j = 1:6
|
||||
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);
|
||||
% tau_msrd = horzcat(tau_msrd, diag(drvGains)*vldtnTrjctry.i(i,:)');
|
||||
% tau_OLS = horzcat(tau_OLS, [Ybi Yfrctni]*[pib_OLS; pifrctn_OLS]);
|
||||
% tau_SDP = horzcat(tau_SDP, [Ybi Yfrctni]*[pib_SDP; pifrctn_SDP]);
|
||||
|
||||
i_prdct1 = horzcat(i_prdct1, diag(drvGains)\(tau_withoutFriction + tau_lnr_frcn));
|
||||
% i_prdct2 = horzcat(i_prdct2, diag(drvGains)\(tau_withoutFriction + tau_nonlnr_frcn));
|
||||
i_OLS1 = horzcat(i_OLS1, diag(drvGains)\([Ybi Yfrctni]*[pib_OLS(:,1); pifrctn_OLS(:,1)]));
|
||||
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
|
||||
|
||||
%%
|
||||
plotTorquesWithLinearFriction = 0;
|
||||
plotTorquesWithNonlinearFriction = 0;
|
||||
|
||||
for i = 1:6
|
||||
figure
|
||||
hold on
|
||||
plot(vldtnTrjctry.t, vldtnTrjctry.i(:,i), 'r-')
|
||||
plot(vldtnTrjctry.t, i_prdct1(i,:), 'k-')
|
||||
legend('measured', 'prdctd1')
|
||||
plot(vldtnTrjctry.t, i_OLS1(i,:), 'k-', 'LineWidth',1.5)
|
||||
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
|
||||
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
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue