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

experiment design is changed as to have two options for optimization algorithms - pattern search and genetic algorithm. Generation of full regressor and well as load regressor are put into seperate file

This commit is contained in:
Shamil Mamedov 2019-12-19 09:48:18 +03:00
parent 856c868a51
commit 7e7f3512c4
7 changed files with 301 additions and 281 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
trajectory_optmzn/ptrnSrch_N5T20.mat Normal file
View File

Binary file not shown.

5
ur_base_params_QRlgr.m
View File

@ -129,9 +129,10 @@ wr = invG*we;
% ----------------------------------------------------------------------- % -----------------------------------------------------------------------
fprintf('Validation of mapping from standard parameters to base ones\n') fprintf('Validation of mapping from standard parameters to base ones\n')
if includeMotorDynamics if includeMotorDynamics
ur10.pi = rand(nLnkPrms*nLnks, 1); ur10.pi(end+1,:) = rand(1,nLnks);
ur10.pi = reshape(ur10.pi,[nLnkPrms*nLnks, 1]);
else else
ur10.pi = reshape(ur10.pi,[60,1]); ur10.pi = reshape(ur10.pi,[nLnkPrms*nLnks, 1]);
end end
% On random positions, velocities, aceeleations % On random positions, velocities, aceeleations

175
ur_base_params_QRlgr.m~
View File

@ -1,175 +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;
%}
return
% -----------------------------------------------------------------------
% Validation of obtained mappings
% -----------------------------------------------------------------------
fprintf('Validation of mapping from standard parameters to base ones\n')
ur10.pi = reshape(ur10.pi,[60,1]);
% 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
Yi = full_regressor_UR10E(q_rnd, qd_rnd, q2d_rnd);
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
% -----------------------------------------------------------------------
% 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

95
ur10_base_params_sym.m → ur_base_params_sym.m
View File

@ -1,5 +1,3 @@
% Get robot description
run('main_ur.m')
% ----------------------------------------------------------------------- % -----------------------------------------------------------------------
% %
% Getting base parameters of the UR10 manipulator based on the % Getting base parameters of the UR10 manipulator based on the
@ -11,10 +9,13 @@ run('main_ur.m')
% Here we tried to generilize what was given there to more general % Here we tried to generilize what was given there to more general
% parametrization compared to their modified DH parameters. % parametrization compared to their modified DH parameters.
% ------------------------------------------------------------------------ % ------------------------------------------------------------------------
% Get robot description
run('main_ur.m')
generateBaseRegressorFunction = 0;
generateBaseDynamicsFunctions = 0;
% -----------------------------------------------------------------------
% Create symbolic parameters % Create symbolic parameters
% -----------------------------------------------------------------------
m = sym('m%d',[6,1],'real'); m = sym('m%d',[6,1],'real');
hx = sym('h%d_x',[6,1],'real'); hx = sym('h%d_x',[6,1],'real');
hy = sym('h%d_y',[6,1],'real'); hy = sym('h%d_y',[6,1],'real');
@ -36,9 +37,7 @@ for i = 1:6
hx(i),hy(i),hz(i),m(i)]'; hx(i),hy(i),hz(i),m(i)]';
end end
% ------------------------------------------------------------------------
% Symbolic generilized coordiates, their first and second deriatives % Symbolic generilized coordiates, their first and second deriatives
% -----------------------------------------------------------------------
q_sym = sym('q%d',[6,1],'real'); q_sym = sym('q%d',[6,1],'real');
qd_sym = sym('qd%d',[6,1],'real'); qd_sym = sym('qd%d',[6,1],'real');
q2d_sym = sym('q2d%d',[6,1],'real'); q2d_sym = sym('q2d%d',[6,1],'real');
@ -76,71 +75,13 @@ for i = 1:6
g_kk(:,i+1) = T_pk(1:3,1:3,i)'*g_kk(:,i); g_kk(:,i+1) = T_pk(1:3,1:3,i)'*g_kk(:,i);
p_kk(:,i+1) = T_pk(1:3,1:3,i)'*(p_kk(:,i) + sym(p_pj)); p_kk(:,i+1) = T_pk(1:3,1:3,i)'*(p_kk(:,i) + sym(p_pj));
K_reg(i,:) = [sym(0.5)*w2wtlda(w_kk(:,i+1)),...
v_kk(:,i+1)'*vec2skewSymMat(w_kk(:,i+1)),...
sym(0.5)*v_kk(:,i+1)'*v_kk(:,i+1)];
P_reg(i,:) = [sym(zeros(1,6)), g_kk(:,i+1)',...
g_kk(:,i+1)'*p_kk(:,i+1)];
lmda(:,:,i) = getLambda(T_pk(1:3,1:3,i),sym(p_pj)); lmda(:,:,i) = getLambda(T_pk(1:3,1:3,i),sym(p_pj));
f(:,i) = getF(v_kk(:,i+1),w_kk(:,i+1),sym(jnt_axs_k),qd_sym(i)); f(:,i) = getF(v_kk(:,i+1),w_kk(:,i+1),sym(jnt_axs_k),qd_sym(i));
DK(:,i+1) = lmda(:,:,i)*DK(:,i) + qd_sym(i)*f(:,i); DK(:,i+1) = lmda(:,:,i)*DK(:,i) + qd_sym(i)*f(:,i);
DP(:,i+1) = lmda(:,:,i)*DP(:,i); DP(:,i+1) = lmda(:,:,i)*DP(:,i);
end end
% ---------------------------------------------------------
% Kinetic and potential energy of the load
% ---------------------------------------------------------
% Transformation from link 6 frame to end-effector frame
rpy_ee = sym(str2num(ur10.robot.joint{7}.origin.Attributes.rpy));
R_6ee = RPY(rpy_ee);
R_6ee(abs(R_6ee)<sqrt(eps)) = sym(0); % to avoid numerical errors
p_6ee = str2num(ur10.robot.joint{7}.origin.Attributes.xyz)';
T_6ee = sym([R_6ee, p_6ee; zeros(1,3), 1]); % to avoid numerical errors
w_eeee = T_6ee(1:3,1:3)'*w_kk(:,7);
v_eeee = T_6ee(1:3,1:3)'*(v_kk(:,7) + cross(w_kk(:,i+1),sym(p_6ee)));
g_eeee = T_6ee(1:3,1:3)'*g_kk(:,7);
p_eeee = T_6ee(1:3,1:3)'*(p_kk(:,7) + sym(p_6ee));
K_l = [sym(0.5)*w2wtlda(w_eeee), v_eeee'*vec2skewSymMat(w_eeee),...
sym(0.5)*(v_eeee'*v_eeee)];
P_l = [sym(zeros(1,6)), g_eeee', g_eeee'*p_eeee];
% -----------------------------------------------------
% Dynamic regressor of the load
% -----------------------------------------------------
Lagrl = K_l - P_l;
dLagrl_dq = jacobian(Lagrl,q_sym)';
dLagrl_dqd = jacobian(Lagrl,qd_sym)';
tl = sym(zeros(6,10));
for i = 1:6
tl = tl + diff(dLagrl_dqd,q_sym(i))*qd_sym(i)+...
diff(dLagrl_dqd,qd_sym(i))*q2d_sym(i);
end
Y_l = tl - dLagrl_dq;
% matlabFunction(Y_l,'File','idntfcn/load_regressor_UR10E',...
% 'Vars',{q_sym,qd_sym,q2d_sym});
% ----------------------------------------------------
% Dynamic regressor of the full paramters
% ----------------------------------------------------
Lagrf = [K_reg(1,:) - P_reg(1,:), K_reg(2,:) - P_reg(2,:),...
K_reg(3,:) - P_reg(3,:), K_reg(4,:) - P_reg(4,:),...
K_reg(5,:) - P_reg(5,:), K_reg(6,:) - P_reg(6,:)];
dLagrf_dq = jacobian(Lagrf,q_sym)';
dLagrf_dqd = jacobian(Lagrf,qd_sym)';
tf = sym(zeros(6,60));
for i = 1:6
tf = tf + diff(dLagrf_dqd,q_sym(i))*qd_sym(i)+...
diff(dLagrf_dqd,qd_sym(i))*q2d_sym(i);
end
Y_f = tf - dLagrf_dq;
matlabFunction(Y_f,'File','autogen/full_regressor_UR10E',...
'Vars',{q_sym,qd_sym,q2d_sym});
return
% ----------------------------------------------------------------------- % -----------------------------------------------------------------------
% Regrouping parameters of the links % Regrouping parameters of the links
% ----------------------------------------------------------------------- % -----------------------------------------------------------------------
@ -160,6 +101,7 @@ for i = 6:-1:2
end end
pir_ur10_num = double(pir_ur10_num); pir_ur10_num = double(pir_ur10_num);
% ----------------------------------------------------------------------- % -----------------------------------------------------------------------
% Getting base parameters % Getting base parameters
% ----------------------------------------------------------------------- % -----------------------------------------------------------------------
@ -209,20 +151,23 @@ for i = 1:6
pir_base_vctr = vertcat(pir_base_vctr,pir_base_num{i}); pir_base_vctr = vertcat(pir_base_vctr,pir_base_num{i});
end end
% ----------------------------------------------------------------------- % -----------------------------------------------------------------------
% Computing Regressor % Computing Regressor
% ----------------------------------------------------------------------- % -----------------------------------------------------------------------
dLagr_dq = jacobian(grad_Lagr,q_sym)'; dLagr_dq = jacobian(grad_Lagr,q_sym)';
dLagr_dqd = jacobian(grad_Lagr,qd_sym)'; dLagr_dqd = jacobian(grad_Lagr,qd_sym)';
t1 = sym(zeros(6,length(pir_base_vctr))); t1 = sym(zeros(6,length(pir_base_vctr)));
% t1 = sym(zeros(6,42));
for i = 1:6 for i = 1:6
t1 = t1 + diff(dLagr_dqd,q_sym(i))*qd_sym(i)+... t1 = t1 + diff(dLagr_dqd,q_sym(i))*qd_sym(i)+...
diff(dLagr_dqd,qd_sym(i))*q2d_sym(i); diff(dLagr_dqd,qd_sym(i))*q2d_sym(i);
end end
Y_hat = t1 - dLagr_dq; Y_hat = t1 - dLagr_dq;
% matlabFunction(Y_hat,'File','idntfcn/base_regressor_UR10E',...
% 'Vars',{q_sym,qd_sym,q2d_sym}); if generateBaseRegressorFunction
matlabFunction(Y_hat,'File','autogen/base_regressor_UR10E',...
'Vars',{q_sym,qd_sym,q2d_sym});
end
% ------------------------------------------------------------ % ------------------------------------------------------------
% Finding mapping from standard parameters to base parameters % Finding mapping from standard parameters to base parameters
@ -234,6 +179,7 @@ pi_ur10_base = [pir_base_sym{1};pir_base_sym{2};pir_base_sym{3};...
dbase_dfull = jacobian(pi_ur10_base,pi_ur10_full); dbase_dfull = jacobian(pi_ur10_base,pi_ur10_full);
Kd = jacobian(pi_ur10_base - Pb*pi_ur10_full, Pd*pi_ur10_full); Kd = jacobian(pi_ur10_base - Pb*pi_ur10_full, Pd*pi_ur10_full);
% -------------------------------------------------------------------- % --------------------------------------------------------------------
% Computing matrices of dynamic equations of motion % Computing matrices of dynamic equations of motion
% -------------------------------------------------------------------- % --------------------------------------------------------------------
@ -277,18 +223,19 @@ for i = 1:1:6
end end
end end
% Generating matrices if generateBaseDynamicsFunctions
fprintf('Generating dynamic equatio elements:\n') fprintf('Generating dynamic equation elements:\n');
%{
fprintf('\t Mass matrix\n') fprintf('\t Mass matrix\n');
matlabFunction(M_mtrx_sym,'File','autogen/M_mtrx_fcn','Vars',{q_sym,xi}); matlabFunction(M_mtrx_sym,'File','autogen/M_mtrx_fcn','Vars',{q_sym,xi});
fprintf('\t Matrix of Coriolis and Centrifugal Forces\n') fprintf('\t Matrix of Coriolis and Centrifugal Forces\n');
matlabFunction(C_mtrx_sym,'File','autogen/C_mtrx_fcn','Vars',{q_sym,qd_sym,xi}); matlabFunction(C_mtrx_sym,'File','autogen/C_mtrx_fcn','Vars',{q_sym,qd_sym,xi});
fprintf('\t Vector of gravitational forces\n') fprintf('\t Vector of gravitational forces\n');
matlabFunction(G_vctr_sym,'File','autogen/G_vctr_fcn','Vars',{q_sym,xi}); matlabFunction(G_vctr_sym,'File','autogen/G_vctr_fcn','Vars',{q_sym,xi});
%} end
return return
% -------------------------------------------------------------------- % --------------------------------------------------------------------
% Tests % Tests

60
ur_exprmt_dsgn.m
View File

@ -4,6 +4,9 @@
% --------------------------------------------------------------------- % ---------------------------------------------------------------------
run('main_ur.m'); % get robot description run('main_ur.m'); % get robot description
% Choose optimization algorithm: 'patternsearch', 'ga'
optmznAlgorithm = 'patternsearch';
% Getting limits on posistion and velocities. Moreover we get some % Getting limits on posistion and velocities. Moreover we get some
% constant parmeters of the robot that allow us to accelerate computation % constant parmeters of the robot that allow us to accelerate computation
% of the robot regressor. % of the robot regressor.
@ -38,33 +41,38 @@ traj_par.t = 0:traj_par.t_smp:traj_par.T; % time
traj_par.N = 5; % number of harmonics traj_par.N = 5; % number of harmonics
% ------------------------------------------------------------------------ % ----------------------------------------------------------------------
% Otimization % Otimization
% ------------------------------------------------------------------------ % -----------------------------------------------------------------------
A = []; b = []; A = []; b = [];
Aeq = []; beq = []; Aeq = []; beq = [];
lb = []; ub = []; lb = []; ub = [];
x0 = rand(6*2*traj_par.N,1);
if strcmp(optmznAlgorithm, 'patternsearch')
x0 = rand(6*2*traj_par.N,1);
optns_pttrnSrch = optimoptions('patternsearch'); optns_pttrnSrch = optimoptions('patternsearch');
optns_pttrnSrch.Display = 'iter'; optns_pttrnSrch.Display = 'iter';
optns_pttrnSrch.StepTolerance = 1e-3;
x = patternsearch(@(x)traj_cost_lgr(x,traj_par,ur10),x0,A,b,Aeq,beq,lb,ub,... [x,fval] = patternsearch(@(x)traj_cost_lgr(x,traj_par,ur10), x0, ...
A, b, Aeq, beq, lb, ub, ...
@(x)traj_cnstr(x,traj_par,ur10), optns_pttrnSrch); @(x)traj_cnstr(x,traj_par,ur10), optns_pttrnSrch);
elseif strcmp(optmznAlgorithm, 'ga')
optns_ga = optimoptions('ga');
optns_ga.Display = 'iter';
optns_ga.PlotFcn = 'gaplotbestf'; % {'gaplotbestf', 'gaplotscores'}
optns_ga.MaxGenerations = 50;
optns_ga.PopulationSize = 1e+3; % in each generation.
optns_ga.InitialPopulationRange = [-100; 100];
optns_ga.SelectionFcn = 'selectionroulette';
return [x,fval] = ga(@(x)traj_cost_lgr(x,traj_par,ur10), 6*2*traj_par.N,...
A, b, Aeq, beq, lb, ub, ...
options = optimoptions('ga'); @(x)traj_cnstr(x,traj_par,ur10),optns_ga);
options.Display = 'iter'; else
options.PlotFcn = 'gaplotbestf'; % {'gaplotbestf', 'gaplotscores'} error('Chosen algorithm is not found among implemented ones');
options.MaxGenerations = 50; end
options.PopulationSize = 1e+3; % in each generation.
options.InitialPopulationRange = [-100; 100];
options.SelectionFcn = 'selectionroulette';
[x,fval,exitflag,output,population,scores] = ga(@(x)traj_cost(x,traj_par,ur10),...
6*2*traj_par.N,A,b,Aeq,beq,lb,ub,@(x)traj_cnstr(x,traj_par,ur10),options);
%% ------------------------------------------------------------------------ %% ------------------------------------------------------------------------
% Verifying obtained trajectory % Verifying obtained trajectory
@ -72,18 +80,34 @@ options.SelectionFcn = 'selectionroulette';
ab = reshape(x,[12,traj_par.N]); ab = reshape(x,[12,traj_par.N]);
a = ab(1:6,:); % sin coeffs a = ab(1:6,:); % sin coeffs
b = ab(7:12,:); % cos coeffs b = ab(7:12,:); % cos coeffs
c_pol = pol_coeffs(traj_par.T,a,b,traj_par.wf,traj_par.N); % polynomial coeffs c_pol = getPolCoeffs(traj_par.T, a, b, traj_par.wf, traj_par.N, ur10.q0);
[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);
traj_cnstr(x,traj_par,ur10)
figure figure
subplot(3,1,1) subplot(3,1,1)
plot(traj_par.t,q) plot(traj_par.t,q)
ylabel('$q$','interpreter','latex')
grid on
legend('q1','q2','q3','q4','q5','q6') legend('q1','q2','q3','q4','q5','q6')
subplot(3,1,2) subplot(3,1,2)
plot(traj_par.t,qd) plot(traj_par.t,qd)
ylabel('$\dot{q}$','interpreter','latex')
grid on
legend('qd1','qd2','qd3','qd4','qd5','qd6') legend('qd1','qd2','qd3','qd4','qd5','qd6')
subplot(3,1,3) subplot(3,1,3)
plot(traj_par.t,q2d) plot(traj_par.t,q2d)
ylabel('$\ddot{q}$','interpreter','latex')
grid on
legend('q2d1','q2d2','q2d3','q2d4','q2d5','q2d6') legend('q2d1','q2d2','q2d3','q2d4','q2d5','q2d6')
pathToFolder = 'trajectory_optmzn/';
if strcmp(optmznAlgorithm, 'patternsearch')
t1 = strcat('N',num2str(traj_par.N),'T',num2str(traj_par.T));
filename = strcat(pathToFolder,'ptrnSrch_',t1,'.mat');
elseif strcmp(optmznAlgorithm, 'ga')
t1 = strcat('N',num2str(traj_par.N),'T',num2str(traj_par.T));
filename = strcat(pathToFolder,'ga_',t1,'.mat');
end
save(filename,'a','b','c_pol','traj_par')

108
ur_exprmt_dsgn.m~ Normal file
View File

@ -0,0 +1,108 @@
% ---------------------------------------------------------------------
% In this script trajectory optimization otherwise called experiment
% design for dynamic paramters identification is carried out.
% ---------------------------------------------------------------------
run('main_ur.m'); % get robot description
% Choose optimization algorithm: 'patternsearch', 'ga'
optmznAlgorithm = 'patternsearch';
% Getting limits on posistion and velocities. Moreover we get some
% constant parmeters of the robot that allow us to accelerate computation
% of the robot regressor.
q_min = zeros(6,1); q_max = zeros(6,1); qd_max = zeros(6,1);
for i = 1:6
rot_axes(:,i) = str2num(ur10.robot.joint{i}.axis.Attributes.xyz)';
R_pj = RPY(str2num(ur10.robot.joint{i}.origin.Attributes.rpy));
p_pj = str2num(ur10.robot.joint{i}.origin.Attributes.xyz)';
T_pj(:,:,i) = [R_pj, p_pj; zeros(1,3), 1];
r_com(:,i) = str2num(ur10.robot.link{i+1}.inertial.origin.Attributes.xyz)';
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
ur10.rot_axes = rot_axes; % axis of rotation of each joint
ur10.T_pj = T_pj;
ur10.r_com = r_com;
ur10.qd_max = qd_max;
ur10.q2d_max = 2*ones(6,1);
% Use different limit for positions for safety
ur10.q_min = deg2rad([-180 -180 -90 -180 -90 -90]');
ur10.q_max = deg2rad([180 0 90 0 90 90]');
ur10.q0 = deg2rad([0 -90 0 -90 0 0 ]');
% Trajectory parameters
traj_par.T = 20; % period of signal
traj_par.wf = 2*pi/traj_par.T; % fundamental frequency
traj_par.t_smp = 1e-1; % sampling time
traj_par.t = 0:traj_par.t_smp:traj_par.T; % time
traj_par.N = 5; % number of harmonics
% ----------------------------------------------------------------------
% Otimization
% -----------------------------------------------------------------------
A = []; b = [];
Aeq = []; beq = [];
lb = []; ub = [];
if strcmp(optmznAlgorithm, 'patternsearch')
x0 = rand(6*2*traj_par.N,1);
optns_pttrnSrch = optimoptions('patternsearch');
optns_pttrnSrch.Display = 'iter';
optns_pttrnSrch.StepTolerance = 1e-3;
[x,fval] = patternsearch(@(x)traj_cost_lgr(x,traj_par,ur10), x0, ...
A, b, Aeq, beq, lb, ub, ...
@(x)traj_cnstr(x,traj_par,ur10), optns_pttrnSrch);
elseif strcmp(optmznAlgorithm, 'ga')
optns_ga = optimoptions('ga');
optns_ga.Display = 'iter';
optns_ga.PlotFcn = 'gaplotbestf'; % {'gaplotbestf', 'gaplotscores'}
optns_ga.MaxGenerations = 50;
optns_ga.PopulationSize = 1e+3; % in each generation.
optns_ga.InitialPopulationRange = [-100; 100];
optns_ga.SelectionFcn = 'selectionroulette';
[x,fval] = ga(@(x)traj_cost_lgr(x,traj_par,ur10), 6*2*traj_par.N,...
A, b, Aeq, beq, lb, ub, ...
@(x)traj_cnstr(x,traj_par,ur10),optns_ga);
else
error('Chosen algorithm is not found among implemented ones');
end
%% ------------------------------------------------------------------------
% Verifying obtained trajectory
% ------------------------------------------------------------------------
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);
[q,qd,q2d] = mixed_traj(traj_par.t, c_pol, a, b, traj_par.wf, traj_par.N);
if strcmp(optmznAlgorithm, 'patternsearch')
strcat('N',num2str(traj_par.N),'T',num2str(traj_par.T))
elseif strcmp(optmznAlgorithm, 'ga')
end
figure
subplot(3,1,1)
plot(traj_par.t,q)
ylabel('$q$','interpreter','latex')
grid on
legend('q1','q2','q3','q4','q5','q6')
subplot(3,1,2)
plot(traj_par.t,qd)
ylabel('$\dot{q}$','interpreter','latex')
grid on
legend('qd1','qd2','qd3','qd4','qd5','qd6')
subplot(3,1,3)
plot(traj_par.t,q2d)
ylabel('$\ddot{q}$','interpreter','latex')
grid on
legend('q2d1','q2d2','q2d3','q2d4','q2d5','q2d6')

115
ur_regressors_lgr.m Normal file
View File

@ -0,0 +1,115 @@
% ----------------------------------------------------------------------
% In this file full regressor of the robot as well as load regressor
% are obtained using Lagrange formulation of dynamics.
% ----------------------------------------------------------------------
% Get robot description
run('main_ur.m')
generateLoadRegressorFunction = 0;
generateFullRegressorFunction = 0;
% Symbolic generilized coordiates, their first and second deriatives
q_sym = sym('q%d',[6,1],'real');
qd_sym = sym('qd%d',[6,1],'real');
q2d_sym = sym('q2d%d',[6,1],'real');
% ------------------------------------------------------------------------
% Getting gradient of energy functions, to derive dynamics
% ------------------------------------------------------------------------
T_pk = sym(zeros(4,4,6)); % transformation between links
w_kk(:,1) = sym(zeros(3,1)); % angular velocity k in frame k
v_kk(:,1) = sym(zeros(3,1)); % linear velocity of the origin of frame k in frame k
g_kk(:,1) = sym([0,0,9.81])'; % vector of graviatational accelerations in frame k
p_kk(:,1) = sym(zeros(3,1)); % origin of frame k in frame k
DK(:,1) = sym(zeros(10,1)); % gradient of kinetic energy
DP(:,1) = sym([zeros(1,6),[0,0,9.81],0]'); % gradient of gravitational energy
for i = 1:6
jnt_axs_k = str2num(ur10.robot.joint{i}.axis.Attributes.xyz)';
% Transformation from parent link frame p to current joint frame
rpy_k = sym(str2num(ur10.robot.joint{i}.origin.Attributes.rpy));
R_pj = RPY(rpy_k);
R_pj(abs(R_pj)<sqrt(eps)) = sym(0); % to avoid numerical errors
p_pj = str2num(ur10.robot.joint{i}.origin.Attributes.xyz)';
T_pj = sym([R_pj, p_pj; zeros(1,3), 1]); % to avoid numerical errors
% Tranformation from joint frame of the joint that rotaties body k to
% link frame. The transformation is pure rotation
R_jk = Rot(q_sym(i),sym(jnt_axs_k));
p_jk = sym(zeros(3,1));
T_jk = [R_jk, p_jk; sym(zeros(1,3)),sym(1)];
% Transformation from parent link frame p to current link frame k
T_pk(:,:,i) = T_pj*T_jk;
z_kk(:,i) = sym(jnt_axs_k);
w_kk(:,i+1) = T_pk(1:3,1:3,i)'*w_kk(:,i) + sym(jnt_axs_k)*qd_sym(i);
v_kk(:,i+1) = T_pk(1:3,1:3,i)'*(v_kk(:,i) + cross(w_kk(:,i),sym(p_pj)));
g_kk(:,i+1) = T_pk(1:3,1:3,i)'*g_kk(:,i);
p_kk(:,i+1) = T_pk(1:3,1:3,i)'*(p_kk(:,i) + sym(p_pj));
beta_K(i,:) = [sym(0.5)*w2wtlda(w_kk(:,i+1)),...
v_kk(:,i+1)'*vec2skewSymMat(w_kk(:,i+1)),...
sym(0.5)*v_kk(:,i+1)'*v_kk(:,i+1)];
beta_P(i,:) = [sym(zeros(1,6)), g_kk(:,i+1)',...
g_kk(:,i+1)'*p_kk(:,i+1)];
end
% --------------------------------------------------------------------
% Gradient of the kinetic and potential energy of the load
% --------------------------------------------------------------------
% Transformation from link 6 frame to end-effector frame
rpy_ee = sym(str2num(ur10.robot.joint{7}.origin.Attributes.rpy));
R_6ee = RPY(rpy_ee);
R_6ee(abs(R_6ee)<sqrt(eps)) = sym(0); % to avoid numerical errors
p_6ee = str2num(ur10.robot.joint{7}.origin.Attributes.xyz)';
T_6ee = sym([R_6ee, p_6ee; zeros(1,3), 1]); % to avoid numerical errors
w_eeee = T_6ee(1:3,1:3)'*w_kk(:,7);
v_eeee = T_6ee(1:3,1:3)'*(v_kk(:,7) + cross(w_kk(:,i+1),sym(p_6ee)));
g_eeee = T_6ee(1:3,1:3)'*g_kk(:,7);
p_eeee = T_6ee(1:3,1:3)'*(p_kk(:,7) + sym(p_6ee));
beta_Kl = [sym(0.5)*w2wtlda(w_eeee), v_eeee'*vec2skewSymMat(w_eeee),...
sym(0.5)*(v_eeee'*v_eeee)];
beta_Pl = [sym(zeros(1,6)), g_eeee', g_eeee'*p_eeee];
% ---------------------------------------------------------------------
% Dynamic regressor of the load
% ---------------------------------------------------------------------
Lagrl = beta_Kl - beta_Pl;
dLagrl_dq = jacobian(Lagrl,q_sym)';
dLagrl_dqd = jacobian(Lagrl,qd_sym)';
tl = sym(zeros(6,10));
for i = 1:6
tl = tl + diff(dLagrl_dqd,q_sym(i))*qd_sym(i)+...
diff(dLagrl_dqd,qd_sym(i))*q2d_sym(i);
end
Y_l = tl - dLagrl_dq;
if generateLoadRegressorFunction
matlabFunction(Y_l,'File','autogen/load_regressor_UR10E',...
'Vars',{q_sym,qd_sym,q2d_sym});
end
% ---------------------------------------------------------------------
% Dynamic regressor of the full paramters
% ---------------------------------------------------------------------
Lagrf = [beta_K(1,:) - beta_P(1,:), beta_K(2,:) - beta_P(2,:),...
beta_K(3,:) - beta_P(3,:), beta_K(4,:) - beta_P(4,:),...
beta_K(5,:) - beta_P(5,:), beta_K(6,:) - beta_P(6,:)];
dLagrf_dq = jacobian(Lagrf,q_sym)';
dLagrf_dqd = jacobian(Lagrf,qd_sym)';
tf = sym(zeros(6,60));
for i = 1:6
tf = tf + diff(dLagrf_dqd,q_sym(i))*qd_sym(i)+...
diff(dLagrf_dqd,qd_sym(i))*q2d_sym(i);
end
Y_f = tf - dLagrf_dq;
if generateFullRegressorFunction
matlabFunction(Y_f,'File','autogen/full_regressor_UR10E',...
'Vars',{q_sym,qd_sym,q2d_sym});
end