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

@@ -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

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

ur_base_params_QRlgr.m~

@@ -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

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,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,'QR.mat');
-    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');
+elseif strcmp(optmznAlgorithm, 'fmincon')
+    filename = strcat(pathToFolder,'fmncn_',t1,'.mat');
 end
 save(filename,'a','b','c_pol','traj_par')