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 0000000..73339f5 Binary files /dev/null and b/trajectory_optmzn/fmncn_N5T20.mat differ diff --git a/trajectory_optmzn/ptrnSrch_N5T20QR.mat b/trajectory_optmzn/ptrnSrch_N5T20QR.mat new file mode 100644 index 0000000..cf8af31 Binary files /dev/null and b/trajectory_optmzn/ptrnSrch_N5T20QR.mat differ 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')