From 7e7f3512c43fcdcaaab494334c09cfad06da8e38 Mon Sep 17 00:00:00 2001 From: Shamil Mamedov Date: Thu, 19 Dec 2019 09:48:18 +0300 Subject: [PATCH] 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 --- trajectory_optmzn/ptrnSrch_N5T20.mat | Bin 0 -> 1680 bytes ur_base_params_QRlgr.m | 5 +- ur_base_params_QRlgr.m~ | 175 ------------------ ..._base_params_sym.m => ur_base_params_sym.m | 103 +++-------- ur_exprmt_dsgn.m | 76 +++++--- ur_exprmt_dsgn.m~ | 108 +++++++++++ ur_regressors_lgr.m | 115 ++++++++++++ 7 files changed, 301 insertions(+), 281 deletions(-) create mode 100644 trajectory_optmzn/ptrnSrch_N5T20.mat delete mode 100644 ur_base_params_QRlgr.m~ rename ur10_base_params_sym.m => ur_base_params_sym.m (73%) create mode 100644 ur_exprmt_dsgn.m~ create mode 100644 ur_regressors_lgr.m diff --git a/trajectory_optmzn/ptrnSrch_N5T20.mat b/trajectory_optmzn/ptrnSrch_N5T20.mat new file mode 100644 index 0000000000000000000000000000000000000000..82df1f60775271ae70f2ff52f8f723c8a719960e GIT binary patch literal 1680 zcma)*TUgQu7{)a#+}0#T-NFNGO>Lp5St_KGZO)aa<}5Ao(2OYY0Ak?@XeBvPE9VYw zqRx7pX%i3B{Chyf6PZk>wh?MZVv&g=qF@1kR?nVxv77CEFTVG?_-=mB`-KHYg$C|N z?m>GX!vdq+PM%I7xFQdw9M3$No{@@028V{n1bXjwMShe)IG#y3flN=sBBM^7L4Hg~ zLV9A59vH0WZtN}(?F;W2pNI@K+_}K z?B5Jyb>b$s`C9)HBfWj_QG)?q`kY!UuKHCwu4@mPRJ>peY||IV?FA*Ru4v<4S-*>} zt-P3sV62O}E@W#34iX=YHea8O>Rvs^&ku23xJH63b{AIa7??R0ohI36Vf!Rl<22*m zCU1GjODH+vK^fx+HP9Ak&b*?%nHeNW|1Dz(rKuO~l8l5@NgB;RWJbz>nJ#Jq`R=uj zU`-<^XX;W0opd6Xm1GsMN`e|_;1;R^NC?Zitku)c*@nDat1kLX)^LC{c0#x&k}FI% zE#>+BH!IV>SwZ!jYquI~?7^87MlS7#t9_4)&ZV!_85K~THdy^61Up^z+e(Gkv9U*s zP8BcMo+%bE=jCMM?>VUnc`H@5cUQmHd>MQ`EC1-_shgO_bwj?o*f4}ThGQBSB;%?K zU(owDQ5(#RjE|{T31*(jTUO6eqZg;5ThQ~d)Lzi8c8`31)0BdCggEq;B0Ht~eBH^R z(Hfzmd=$@Ag7O}4eNtl8oNz_SB;;0qDqU_ibbfwceUFniC8r5L1>fmPWiHqK^lj1d zkb!|XNq=}evnSpVX$&f{kt;BfRu=0<+zDm8_XAFJ_Dwo<&AL=qO1lq1ptTcmnWK&- z28V3^D^^GU>D5>N>XpdcCgigFol(IxuyNjn*$DcmFWLOzIZdDI$*G?VYIq3Dl;YE4))_|QE z)L{@9Si^7LLei>TVQ*fZ>LlZaEBX5oIdnx0A%o;VJ2eH_23j?Bp;DOyZl9{Kx6=&h7rQ=J}X+ znELvV#mtY_A*wmLV*E>F(NXU!pq-_$uu>1(Y`Kx2WoK?G==<0*$g3>#oHZgSqBUzd)7V**Ox94{2BL~Hw8&tOAva$STY zf}QQ1y)PlV&0nR(44^8vw`lLM;cXqplGr2$Lp9r<+FcRvWAr;H9(Fa}$~pros`Px^ z)5t5Gm0{ZRnfyoKgQ$Ay6J>1yFVl&`EbF>7v1UmV=wYCjJmC&~fZj(J8hQ-*uecNcHmpG(=>d^dC4}-q4jb{F-o>1-6=w?A1rf#*v)y9 z594j3YlyH`_RUAh06gujR@P0~GXR@r*IG=H!hy|PHfi=4V9RBZWRrje_rikoEO{@0 z<`yo@o~3m7vL1#AP#n~pH6AMNTO<49p|!J7oJ*fmc81I7yEzvp$nfym2&k9$L|;)* zPFZt|aGS3gQM%>D7Cb^MT%*wO2%ny}$!60gF9+qL>8nc5Im1O?7;!d~!}g+VpEmdD zEhca0uy|zKk=~<#XIhb+Y!L7q8I<_i5oIZy3skbjhydgJj(ir+DWp;=IX?cKF3g8I zM@&RJ5ioD<82~oAIIo61E&tu8I( 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 \ No newline at end of file diff --git a/ur10_base_params_sym.m b/ur_base_params_sym.m similarity index 73% rename from ur10_base_params_sym.m rename to ur_base_params_sym.m index b3e1dbb..c0a22b7 100644 --- a/ur10_base_params_sym.m +++ b/ur_base_params_sym.m @@ -1,5 +1,3 @@ -% Get robot description -run('main_ur.m') % ----------------------------------------------------------------------- % % 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 % parametrization compared to their modified DH parameters. % ------------------------------------------------------------------------ +% Get robot description +run('main_ur.m') + +generateBaseRegressorFunction = 0; +generateBaseDynamicsFunctions = 0; -% ----------------------------------------------------------------------- % Create symbolic parameters -% ----------------------------------------------------------------------- m = sym('m%d',[6,1],'real'); hx = sym('h%d_x',[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)]'; end -% ------------------------------------------------------------------------ % 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'); @@ -75,72 +74,14 @@ for i = 1:6 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)); - - 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)); 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); DP(:,i+1) = lmda(:,:,i)*DP(:,i); 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)