From 6065d44dbd9cca28d90441aa175074efa8b03ca1 Mon Sep 17 00:00:00 2001 From: Shamil Mamedov Date: Mon, 23 Dec 2019 08:55:18 +0300 Subject: [PATCH] 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 --- frictionRegressor.m | 32 +++++ trajectory_optmzn/fmncn_N5T20.mat | Bin 0 -> 1784 bytes trajectory_optmzn/ptrnSrch_N5T20QR.mat | Bin 0 -> 1776 bytes trajectory_optmzn/traj_cost_lgr.m | 6 +- ur_base_params_QRlgr.m~ | 181 ------------------------- ur_exprmt_dsgn.m | 34 +++-- 6 files changed, 62 insertions(+), 191 deletions(-) create mode 100644 frictionRegressor.m create mode 100644 trajectory_optmzn/fmncn_N5T20.mat create mode 100644 trajectory_optmzn/ptrnSrch_N5T20QR.mat delete mode 100644 ur_base_params_QRlgr.m~ diff --git a/frictionRegressor.m b/frictionRegressor.m new file mode 100644 index 0000000..3dbac8b --- /dev/null +++ b/frictionRegressor.m @@ -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 \ No newline at end of file diff --git a/trajectory_optmzn/fmncn_N5T20.mat b/trajectory_optmzn/fmncn_N5T20.mat new file mode 100644 index 0000000000000000000000000000000000000000..73339f5f25eee71d5e128318fe2dd406cacaef56 GIT binary patch literal 1784 zcma)*Sya;p8pTfc&fM?e-g6&+=et3^;irB5 zpeNvt&>-J%`{eT(NynjIXW(*@30avQQ2*1xXMGW_$Dt>)l5jamiBJOG19~d!JoL+? z1gNtk)X~l3gu90`VkuJUuBvBM!>DDR zOx^fGnvp73nTRy^q{ZCl>zr)gW`!{tnk*}etoqrOt*rBf7hsOTJ6*>@EXiBurFdm3%^3_OkmeAaT*hS6 zdl83PnoVym+4mXd@6;5;Lo^BgC&X_zluj3NA+<=+D}Yb@F|_yfP~nY@ zdr|qcq=P^>)QFIE^y9!7^0XGay&VUMR$x!gk6MH8EE4z*rW;Ew@6X+d6h@`$Hnm_e z0LQ*G)ALQc)p;`gCt#sl$L!f0X&RW<3ivF8#UkDYoyT)RZ7+eW5gj zvGc+sndP4E7DQ{CbgtXZ#Idy{2VKv<9)LhInC+ccOfYrcxOLZiO8$})N05)O!J9!} zy{q$(h_yeNJ9)sb14>l9(=V!3?VvVxm~bnry?ctczR&;BlO|(OQ@GOoanch>+n_EPlAC;qHhejo&60;auuOhhA)YSp zqF1-gX9929mG#IVCyTRPqcf`ZZ;ngqq)P8IGy)85yG%~`1xLwo&7#j=tWcptDwKj9 zh_v94K#>@^pufnEkbx!Y!rdTG^F$3G%&HTbyRA149{dqVg3zc)1a_DoY%7+zY) zGj)vN)JJiMxa+W2)>nH|!(L3B;^s(-_e_gn4%^CbcA(hKov?vvWNZUym?IyQ% zr4545tgl|T8f)0r)|D&2*xW#4&6FcO?J1Im=GvSB_#`uX`=SW3yC$3^{}r`HISV=L zLvQ+0oK~VlxeUD@eJdsp%TYBl9s(`MO7wn)2+!`sX@e`%22|8#{NzDvB`USWc>Y6? zXQIYM`xzK31FwLkR+3U8iijflF&6z8Gd~T|xNvYpR^d+E$uwW+YY?=P@JwT}K6y90 zD7r7Ek3wRQd?sl506y~{gKPE@wHsBm8Cho?3V#!C7oG>PqkMBt)NF>ZbOMwk>JElTxcQvFHXmYq3I2XpTfL{)r8|Apd zZv;w4IVKBFXPr4F@JoXgjB)#k;kWx-T08jLqA)udkEKL+MHa^kesfTw;CS8t#%^{8 z$6(7|U*jB=hrYRh#%z!5t0vlPS${Pt3G;bme*|$a#wfBkf*66>8_AC#MPN)LACD7H zY?)x3l#4kODfp|K(a*CM%x}{N)`^DVGM;>W8_iVKIBSA2L-yPt{t$qz?1WK*6m@Ur z{;1H{lFoe;d&RjwD?gi%|D0LMYyn3fUu15jIw}sz7A|qUe-_#V7jAs0inb2tc2KGg zZ0Ha`DH^a|^=i>JOSH08k`4vj7IqdRJb?)`P?@iKKT*Q*M|E8Z8DnV~Lq39$x>pDM L)CRlqYn=WK{%L^; literal 0 HcmV?d00001 diff --git a/trajectory_optmzn/ptrnSrch_N5T20QR.mat b/trajectory_optmzn/ptrnSrch_N5T20QR.mat new file mode 100644 index 0000000000000000000000000000000000000000..cf8af31268210cc697708aa81f4ea6944daeeda4 GIT binary patch literal 1776 zcma)*eKgyL8pqM9TdytBC|YS%Y;8r+rXFeK{%d6I5|Qb&p5b1{$e!#h~`CKqYt~ariMm(wKys=@~g<`$jCihOZ@}pdxJ~2 zAaLsgP_0-$h+1P{$=R&oOZT+o*<%Rmi401oSK<@8#Jz@WsF<(ufrpwQ;^%Rg=Qk;LpF#QM_Z31>UV)j_60 zRnL_%ys$gd$ee7JLN`QegYq+43xoKkNF7I&2U=yfg*}_!JL9_^% zLh%sXX``)bWmjHm=FxHqdnz|Uh zcH@nZDW{J;{U#73`<3cv@4O{=Cs1ElX$0Bs0`$c2*w>uLJ5SYWZ$jbDN$@6k|M-+T zbM2uDgz0Z6$#HE@BVtD<*2RTx!qxL-@i6d;5sm7rpGlljkC8?_GOSwWSSMepenxGy zWL*!tSKn8sr}POT)OY`LRAYYw`H<~LCN#O#pEYTi7PXH8giGoqKJ3}RA%p|Lv+n8*I= zVk!0GPp#e_^w=@jv_``$@>NDI@B1v5EjjJbcq4?2@L5MgELD~|zK4sE4T7|*jXrT# z(ZW*lCvQ!+R?UU#ey(%6B%dTO2}HbGUNT+*=IoUdoTy)b@7L2=UcKzcPPvtaHycH# zznf%q=G6DzG9qgg>5xzCxS@aSO?d+F21z0gT3Mm0pCQZnttDOE>`x?bZI!BhCYR%! zkIoMN8w!``pxR0jue3EUZXQuS^cxk(-={4bJq?An+hp+61C0l41*)pev^CddemU*h zZs)5q`=CM}dBg^C*!V|>tQ?-2m|A>}ApgGSu#?r1)(#ZywRC>mMI@j1pIE|S@Ex-I zCJII4qbik@8*2=MV&ob@bQ1HUV4Bv)ul*O#C(-z8v!T^JTDSjRcP{|Y0@ z$~P>gYxcEur^;?M)nw5HrRR}$L~%{i{ge#YOm*`9B|iLMWguPl6tzwawmRybQ+Gjl zog_y&jjeLu2-C)8s7fgd?me6o?e+{FnA~+$?RWS3h$3?ZH)ClnM_r@pyxPh|Mys6E zdddi?@>?a@LQKY^@F;?3#gb=*pq`z@gef`FG8epIwTYy!X817ySFMw#nRc)v_ex(_ zACXW&aOY*EQ*a57*z0!U9cL8BpQDS@20zipYw}OxK;Qvwf(PFmX8`WY!yg08z(d~n zgTP+!pf|w*&^al2$`^xb)A23u-hyf0xhl21RKpWi~gbxBh=)oXD5MY4re~v%*0mIJ;slXBR z@Tbn&4GX5#x8p`O@Oy-JSh9_MSq9P{7W4t5^G}NLKV1yJ+XW^1u041u`lIp=Cv_bn z+O38DS$W#{|96bA3mFVeT&{_Q;<(T^d19IA)-Yk?m%X`l?}~LGvy*twd~+90A$wD- z-@8)PO=7k5Jz*C`sh#M`ha)LGZ_3?t#VU`)_yX1at^YJ#T`}f9%z5y_+;d0kKzgOa E?=PxyVE_OC literal 0 HcmV?d00001 diff --git a/trajectory_optmzn/traj_cost_lgr.m b/trajectory_optmzn/traj_cost_lgr.m index 6139bec..94994c1 100644 --- a/trajectory_optmzn/traj_cost_lgr.m +++ b/trajectory_optmzn/traj_cost_lgr.m @@ -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 diff --git a/ur_base_params_QRlgr.m~ b/ur_base_params_QRlgr.m~ deleted file mode 100644 index 4411e0b..0000000 --- a/ur_base_params_QRlgr.m~ +++ /dev/null @@ -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) 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 \ No newline at end of file diff --git a/ur_exprmt_dsgn.m b/ur_exprmt_dsgn.m index 0540cb6..0aab9a5 100644 --- a/ur_exprmt_dsgn.m +++ b/ur_exprmt_dsgn.m @@ -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,7 +54,10 @@ 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, ... @@ -71,21 +74,35 @@ 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); + @(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 - + %% ------------------------------------------------------------------------ % 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); +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')