New trajectory is found. The difference from previous ones is the fact that now the condition number of the whole whole regressor (with frinction and motor dynamics) is taken into accound
This commit is contained in:
Shamil Mamedov
parent
a5240b3574
commit
6065d44dbd
|
|
@ -0,0 +1,32 @@
|
|||
function frctn = frictionRegressor(qd_fltrd)
|
||||
% ----------------------------------------------------------------------
|
||||
% The function computes friction regressor for each joint of the robot.
|
||||
% There are two models, first one is discontinous,
|
||||
% Fv*qd + Fc*sign(qd) + F0, and the second one is continous,
|
||||
% Fv*qd + Fc*tanh(qd/xi) + F0, where xi is small constant
|
||||
% ---------------------------------------------------------------------
|
||||
frctn = zeros(6,18);
|
||||
for i = 1:6
|
||||
frctn(i,3*i-2:3*i) = [qd_fltrd(i), sign(qd_fltrd(i)), 1];
|
||||
end
|
||||
|
||||
% xi = 1e-4;
|
||||
% cont_Clmb_frcn = 0;
|
||||
%
|
||||
% frctn = zeros(6,3);
|
||||
% if cont_Clmb_frcn
|
||||
% for i = 1:6
|
||||
% frctn(i,:) = [qd_fltrd(i), tanh(qd_fltrd(i)/xi), 1];
|
||||
% end
|
||||
% else
|
||||
% for i = 1:6
|
||||
% frctn(i,:) = [qd_fltrd(i), sign(qd_fltrd(i)), 1];
|
||||
% end
|
||||
% end
|
||||
|
||||
% for i = 1:6
|
||||
% frctn(i,3*i-2:3*i) = [qd_fltrd(i), sign(qd_fltrd(i)), 1];
|
||||
% end
|
||||
% for i = 1:6
|
||||
% frctn(i,3*i-2:3*i) = [qd_fltrd(i), tanh(qd_fltrd(i)/1e-2), 1];
|
||||
% end
|
||||
Binary file not shown.
Binary file not shown.
|
|
@ -33,9 +33,11 @@ W = [];
|
|||
for i = 1:length(t)
|
||||
% Y = base_regressor_UR10E(q(:,i),qd(:,i),q2d(:,i));
|
||||
if baseQR.motorDynamicsIncluded
|
||||
Y = regressorWithMotorDynamics(q(:,i),qd(:,i),q2d(:,i))*E1;
|
||||
Y = [regressorWithMotorDynamics(q(:,i),qd(:,i),q2d(:,i))*E1, ...
|
||||
frictionRegressor(qd(:,i))];
|
||||
else
|
||||
Y = full_regressor_UR10E(q(:,i),qd(:,i),q2d(:,i))*E1;
|
||||
Y = [full_regressor_UR10E(q(:,i),qd(:,i),q2d(:,i))*E1, ...
|
||||
frictionRegressor(qd(:,i))];
|
||||
end
|
||||
W = vertcat(W,Y);
|
||||
end
|
||||
|
|
|
|||
|
|
@ -1,181 +0,0 @@
|
|||
% ----------------------------------------------------------------------
|
||||
% In this script QR decomposition is applied to regressor in closed
|
||||
% form obtained from Lagrange formulation of dynamics.
|
||||
% ----------------------------------------------------------------------
|
||||
% Get robot description
|
||||
run('main_ur.m')
|
||||
|
||||
% Seed the random number generator based on the current time
|
||||
rng('shuffle');
|
||||
|
||||
includeMotorDynamics = 1;
|
||||
|
||||
% ------------------------------------------------------------------------
|
||||
% Getting limits on posistion and velocities
|
||||
% ------------------------------------------------------------------------
|
||||
q_min = zeros(6,1);
|
||||
q_max = zeros(6,1);
|
||||
qd_max = zeros(6,1);
|
||||
q2d_max = 2*ones(6,1); % it is chosen by us as it is not given in URDF
|
||||
for i = 1:6
|
||||
q_min(i) = str2double(ur10.robot.joint{i}.limit.Attributes.lower);
|
||||
q_max(i) = str2double(ur10.robot.joint{i}.limit.Attributes.upper);
|
||||
qd_max(i) = str2double(ur10.robot.joint{i}.limit.Attributes.velocity);
|
||||
end
|
||||
|
||||
|
||||
% -----------------------------------------------------------------------
|
||||
% Standard dynamics paramters of the robot in symbolic form
|
||||
% -----------------------------------------------------------------------
|
||||
m = sym('m%d',[6,1],'real');
|
||||
hx = sym('h%d_x',[6,1],'real');
|
||||
hy = sym('h%d_y',[6,1],'real');
|
||||
hz = sym('h%d_z',[6,1],'real');
|
||||
ixx = sym('i%d_xx',[6,1],'real');
|
||||
ixy = sym('i%d_xy',[6,1],'real');
|
||||
ixz = sym('i%d_xz',[6,1],'real');
|
||||
iyy = sym('i%d_yy',[6,1],'real');
|
||||
iyz = sym('i%d_yz',[6,1],'real');
|
||||
izz = sym('i%d_zz',[6,1],'real');
|
||||
im = sym('im%d',[6,1],'real');
|
||||
|
||||
% Load parameters attached to the end-effector
|
||||
syms ml hl_x hl_y hl_z il_xx il_xy il_xz il_yy il_yz il_zz real
|
||||
|
||||
% Vector of symbolic parameters
|
||||
for i = 1:6
|
||||
if includeMotorDynamics
|
||||
pi_lgr_sym(:,i) = [ixx(i),ixy(i),ixz(i),iyy(i),iyz(i),izz(i),...
|
||||
hx(i),hy(i),hz(i),m(i),im(i)]';
|
||||
else
|
||||
pi_lgr_sym(:,i) = [ixx(i),ixy(i),ixz(i),iyy(i),iyz(i),izz(i),...
|
||||
hx(i),hy(i),hz(i),m(i)]';
|
||||
end
|
||||
end
|
||||
[nLnkPrms, nLnks] = size(pi_lgr_sym);
|
||||
pi_lgr_sym = reshape(pi_lgr_sym, [nLnkPrms*nLnks, 1]);
|
||||
|
||||
|
||||
% -----------------------------------------------------------------------
|
||||
% Find relation between independent columns and dependent columns
|
||||
% -----------------------------------------------------------------------
|
||||
% Get observation matrix of identifiable paramters
|
||||
W = [];
|
||||
for i = 1:20
|
||||
q_rnd = q_min + (q_max - q_min).*rand(6,1);
|
||||
qd_rnd = -qd_max + 2*qd_max.*rand(6,1);
|
||||
q2d_rnd = -q2d_max + 2*q2d_max.*rand(6,1);
|
||||
|
||||
if includeMotorDynamics
|
||||
Y = regressorWithMotorDynamics(q_rnd,qd_rnd,q2d_rnd);
|
||||
else
|
||||
Y = full_regressor_UR10E(q_rnd,qd_rnd,q2d_rnd);
|
||||
end
|
||||
W = vertcat(W,Y);
|
||||
end
|
||||
|
||||
% QR decomposition with pivoting: W*E = Q*R
|
||||
% R is upper triangular matrix
|
||||
% Q is unitary matrix
|
||||
% E is permutation matrix
|
||||
[Q,R,E] = qr(W);
|
||||
|
||||
% matrix W has rank bb which is number number of base parameters
|
||||
bb = rank(W);
|
||||
|
||||
% R = [R1 R2;
|
||||
% 0 0]
|
||||
% R1 is bbxbb upper triangular and reguar matrix
|
||||
% R2 is bbx(c-bb) matrix where c is number of identifiable parameters
|
||||
R1 = R(1:bb,1:bb);
|
||||
R2 = R(1:bb,bb+1:end);
|
||||
beta = R1\R2; % the zero rows of K correspond to independent columns of WP
|
||||
beta(abs(beta)<sqrt(eps)) = 0; % get rid of numerical errors
|
||||
% W2 = W1*beta
|
||||
|
||||
% Make sure that the relation holds
|
||||
W1 = W*E(:,1:bb);
|
||||
W2 = W*E(:,bb+1:end);
|
||||
if norm(W2 - W1*beta) > 1e-6
|
||||
fprintf('Found realationship between W1 and W2 is not correct\n');
|
||||
return
|
||||
end
|
||||
|
||||
|
||||
% -----------------------------------------------------------------------
|
||||
% Find base parmaters
|
||||
% -----------------------------------------------------------------------
|
||||
pi1 = E(:,1:bb)'*pi_lgr_sym; % independent paramters
|
||||
pi2 = E(:,bb+1:end)'*pi_lgr_sym; % dependent paramteres
|
||||
|
||||
% all of the expressions below are equivalent
|
||||
pi_lgr_base = pi1 + beta*pi2;
|
||||
pi_lgr_base2 = [eye(bb) beta]*[pi1;pi2];
|
||||
pi_lgr_base3 = [eye(bb) beta]*E'*pi_lgr_sym;
|
||||
|
||||
% Relationship needed for identifcation using physical feasibility
|
||||
%{
|
||||
KG = [eye(bb) beta; zeros(size(W,2)-bb,bb) eye(size(W,2)-bb)];
|
||||
G = KG*E';
|
||||
invG = E*[eye(bb) -beta; zeros(size(W,2)-bb,bb) eye(size(W,2)-bb)];
|
||||
we = G*pi_lgr_sym;
|
||||
vpa(we,3)
|
||||
wr = invG*we;
|
||||
%}
|
||||
|
||||
% -----------------------------------------------------------------------
|
||||
% Validation of obtained mappings
|
||||
% -----------------------------------------------------------------------
|
||||
fprintf('Validation of mapping from standard parameters to base ones\n')
|
||||
if includeMotorDynamics
|
||||
ur10.pi(end+1,:) = rand(1,nLnks);
|
||||
ur10.pi = reshape(ur10.pi,[nLnkPrms*nLnks, 1]);
|
||||
else
|
||||
ur10.pi = reshape(ur10.pi,[nLnkPrms*nLnks, 1]);
|
||||
end
|
||||
|
||||
% On random positions, velocities, aceeleations
|
||||
for i = 1:100
|
||||
q_rnd = q_min + (q_max - q_min).*rand(6,1);
|
||||
qd_rnd = -qd_max + 2*qd_max.*rand(6,1);
|
||||
q2d_rnd = -q2d_max + 2*q2d_max.*rand(6,1);
|
||||
|
||||
if includeMotorDynamics
|
||||
Yi = regressorWithMotorDynamics(q_rnd,qd_rnd,q2d_rnd);
|
||||
else
|
||||
Yi = full_regressor_UR10E(q_rnd,qd_rnd,q2d_rnd);
|
||||
end
|
||||
tau_full = Yi*ur10.pi;
|
||||
|
||||
pi_lgr_base = [eye(bb) beta]*E'*ur10.pi;
|
||||
Y_base = Yi*E(:,1:bb);
|
||||
tau_base = Y_base*pi_lgr_base;
|
||||
nrm_err1(i) = norm(tau_full - tau_base);
|
||||
end
|
||||
figure
|
||||
plot(nrm_err1)
|
||||
ylabel('||\tau - \tau_b||')
|
||||
grid on
|
||||
|
||||
if ~all(nrm_err1<1e-6)
|
||||
fprintf('Validation failed')
|
||||
return
|
||||
|
||||
|
||||
% -----------------------------------------------------------------------
|
||||
% Additional functions
|
||||
% -----------------------------------------------------------------------
|
||||
function Y = regressorWithMotorDynamics(q,qd,q2d)
|
||||
% ----------------------------------------------------------------------
|
||||
% This function adds motor dynamics to rigid body regressor.
|
||||
% It is simplified model of motor dynamics, it adds only reflected
|
||||
% inertia i.e. I_rflctd = Im*N^2 where N is reduction ratio - I_rflctd*q_2d
|
||||
% parameter is added to existing vector of each link [pi_i I_rflctd_i]
|
||||
% so that each link has 11 parameters
|
||||
% ----------------------------------------------------------------------
|
||||
Y_rgd_bdy = full_regressor_UR10E(q,qd,q2d);
|
||||
Y_mtrs = diag(q2d);
|
||||
Y = [Y_rgd_bdy(:,1:10), Y_mtrs(:,1), Y_rgd_bdy(:,11:20), Y_mtrs(:,2),...
|
||||
Y_rgd_bdy(:,21:30), Y_mtrs(:,3), Y_rgd_bdy(:,31:40), Y_mtrs(:,4),...
|
||||
Y_rgd_bdy(:,41:50), Y_mtrs(:,5), Y_rgd_bdy(:,51:60), Y_mtrs(:,6)];
|
||||
end
|
||||
|
|
@ -6,7 +6,7 @@ run('main_ur.m'); % get robot description
|
|||
|
||||
load('baseQR.mat'); % load mapping from full parameters to base parameters
|
||||
|
||||
% Choose optimization algorithm: 'patternsearch', 'ga'
|
||||
% Choose optimization algorithm: 'patternsearch', 'ga', 'fmincon'
|
||||
optmznAlgorithm = 'patternsearch';
|
||||
|
||||
% Getting limits on posistion and velocities. Moreover we get some
|
||||
|
|
@ -54,8 +54,11 @@ if strcmp(optmznAlgorithm, 'patternsearch')
|
|||
optns_pttrnSrch = optimoptions('patternsearch');
|
||||
optns_pttrnSrch.Display = 'iter';
|
||||
optns_pttrnSrch.StepTolerance = 1e-3;
|
||||
optns_pttrnSrch.FunctionTolerance = 1;
|
||||
optns_pttrnSrch.MaxTime = inf;
|
||||
optns_pttrnSrch.MaxFunctionEvaluations = 1e+6;
|
||||
|
||||
|
||||
[x,fval] = patternsearch(@(x)traj_cost_lgr(x,traj_par,baseQR), x0, ...
|
||||
A, b, Aeq, beq, lb, ub, ...
|
||||
@(x)traj_cnstr(x,traj_par), optns_pttrnSrch);
|
||||
|
|
@ -72,6 +75,21 @@ elseif strcmp(optmznAlgorithm, 'ga')
|
|||
[x,fval] = ga(@(x)traj_cost_lgr(x,traj_par,baseQR), 6*2*traj_par.N,...
|
||||
A, b, Aeq, beq, lb, ub, ...
|
||||
@(x)traj_cnstr(x,traj_par), optns_ga);
|
||||
elseif strcmp(optmznAlgorithm, 'fmincon')
|
||||
x0 = -1 + 2*rand(6*2*traj_par.N,1);
|
||||
optns_fmincon = optimoptions('fmincon');
|
||||
optns_fmincon.Algorithm = 'interior-point';
|
||||
optns_fmincon.Display = 'iter';
|
||||
optns_fmincon.MaxFunctionEvaluations = 1e+5;
|
||||
optns_fmincon.OptimalityTolerance = 1e-3;
|
||||
optns_fmincon.StepTolerance = 1e-10;
|
||||
optns_fmincon.ConstraintTolerance = 1e-3;
|
||||
|
||||
null_cost = @(x) 0;
|
||||
|
||||
[x,fval] = fmincon(@(x)traj_cost_lgr(x,traj_par,baseQR), x0, ...
|
||||
A, b, Aeq, beq, lb, ub, ...
|
||||
@(x)traj_cnstr(x,traj_par), optns_fmincon);
|
||||
else
|
||||
error('Chosen algorithm is not found among implemented ones');
|
||||
end
|
||||
|
|
@ -82,10 +100,9 @@ end
|
|||
ab = reshape(x,[12,traj_par.N]);
|
||||
a = ab(1:6,:); % sin coeffs
|
||||
b = ab(7:12,:); % cos coeffs
|
||||
c_pol = getPolCoeffs(traj_par.T, a, b, traj_par.wf, traj_par.N, ur10.q0);
|
||||
c_pol = getPolCoeffs(traj_par.T, a, b, traj_par.wf, traj_par.N, traj_par.q0);
|
||||
[q,qd,q2d] = mixed_traj(traj_par.t, c_pol, a, b, traj_par.wf, traj_par.N);
|
||||
|
||||
|
||||
figure
|
||||
subplot(3,1,1)
|
||||
plot(traj_par.t,q)
|
||||
|
|
@ -105,11 +122,12 @@ subplot(3,1,3)
|
|||
|
||||
|
||||
pathToFolder = 'trajectory_optmzn/';
|
||||
t1 = strcat('N',num2str(traj_par.N),'T',num2str(traj_par.T));
|
||||
if strcmp(optmznAlgorithm, 'patternsearch')
|
||||
t1 = strcat('N',num2str(traj_par.N),'T',num2str(traj_par.T));
|
||||
filename = strcat(pathToFolder,'ptrnSrch_',t1,'.mat');
|
||||
filename = strcat(pathToFolder,'ptrnSrch_',t1,'QR.mat');
|
||||
elseif strcmp(optmznAlgorithm, 'ga')
|
||||
t1 = strcat('N',num2str(traj_par.N),'T',num2str(traj_par.T));
|
||||
filename = strcat(pathToFolder,'ga_',t1,'.mat');
|
||||
elseif strcmp(optmznAlgorithm, 'fmincon')
|
||||
filename = strcat(pathToFolder,'fmncn_',t1,'.mat');
|
||||
end
|
||||
save(filename,'a','b','c_pol','traj_par')
|
||||
|
|
|
|||
Loading…
Reference in New Issue