system matrices is generated in symbolic form as a function of standard rigid body parameters. Folder are slightly restructured

main_ur.m

@@ -70,7 +70,7 @@ end
 % regressor - Y*pi = tau, and matlab Robotic Systems Toolbox inverse
 % dynamics command.
 % ------------------------------------------------------------------------
-TEST_DYNAMICS = 0;
+TEST_DYNAMICS = 1;
 
 if TEST_DYNAMICS
     rbt = importrobot('ur10e.urdf');
@@ -83,7 +83,7 @@ if TEST_DYNAMICS
     err1 = zeros(noIter,1); err2 = zeros(noIter,1); 
     err3 = zeros(noIter,1); err4 = zeros(noIter,1);
     for i = 1:noIter
-        q = rand(6,1);
+        q = -2*pi + 4*pi*rand(6,1);
         q_d = zeros(6,1);
         q_2d = zeros(6,1);
 
@@ -95,8 +95,10 @@ if TEST_DYNAMICS
         tau2 = Yscrw*reshape(ur10.pi_scrw,[60,1]);
         tau3 = inverseDynamics(rbt,q,q_d,q_2d);
         tau4 = Ylgr*reshape(ur10.pi,[60,1]);
-        tau5 = G_vctr_fcn2(q, reshape(ur10.pi,[60,1]));
-        
+        tau5 = M_mtrx_fcn(q, ur10.pi(:))*q_2d + ...
+               C_mtrx_fcn(q, q_d, ur10.pi(:))*q_d + ...
+               G_vctr_fcn(q, ur10.pi(:));
+                
     %   verifying if regressor is computed correctly  
         err1(i) = norm(tau3 - tau1);
     %   verifying if our inverse dynamics coincides with matlabs

ur_exprmt_dsgn.m

@@ -6,9 +6,8 @@ run('main_ur.m'); % get robot description
 
 load('baseQR.mat'); % load mapping from full parameters to base parameters
 
-% Choose optimization algorithm: 'patternsearch', 'ga', 'fmincon'
+% Choose optimization algorithm: 'patternsearch', 'ga'
 optmznAlgorithm = 'patternsearch';
-searchOnlyForFeasibleSolution = 1;
 
 % Getting limits on posistion and velocities. Moreover we get some
 % constant parmeters of the robot that allow us to accelerate computation
@@ -51,8 +50,7 @@ Aeq = []; beq = [];
 lb = []; ub = [];
 
 if strcmp(optmznAlgorithm, 'patternsearch')
-    load('ptrnSrch_N7T25QR.mat')
-%     x0 = rand(6*2*traj_par.N,1);
+    x0 = rand(6*2*traj_par.N,1);
     x0 = reshape([a b], [6*2*traj_par.N, 1]); 
     optns_pttrnSrch = optimoptions('patternsearch');
     optns_pttrnSrch.Display = 'iter';
@@ -62,15 +60,9 @@ if strcmp(optmznAlgorithm, 'patternsearch')
     optns_pttrnSrch.MaxTime = inf;
     optns_pttrnSrch.MaxFunctionEvaluations = 1e+6;
     
-    if searchOnlyForFeasibleSolution
-    [x,fval] = patternsearch(@(x)0, x0, A, [], Aeq, beq, lb, ub, ...
-                             @(x)traj_cnstr_fsblty(x, traj_par, baseQR), ...
-                             optns_pttrnSrch);    
-    else
     [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);
-    end          
 elseif strcmp(optmznAlgorithm, 'ga')
     optns_ga = optimoptions('ga');
     optns_ga.Display = 'iter';
@@ -83,27 +75,12 @@ 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
 
-%% ------------------------------------------------------------------------
-% Verifying obtained trajectory
+% ------------------------------------------------------------------------
+% Plotting obtained trajectory
 % ------------------------------------------------------------------------
 ab = reshape(x,[12,traj_par.N]);
 a = ab(1:6,:); % sin coeffs
@@ -128,9 +105,11 @@ subplot(3,1,3)
     grid on
     legend('q2d1','q2d2','q2d3','q2d4','q2d5','q2d6')
 
-%%
+% ----------------------------------------------------------------------
+% Saving parameters of the optimized trajectory
+% ----------------------------------------------------------------------
 % %{
-pathToFolder = 'trajectory_optmzn/';
+pathToFolder = 'trajectory_optmzn/optimal_trjctrs/';
 t1 = strcat('N',num2str(traj_par.N),'T',num2str(traj_par.T));
 if strcmp(optmznAlgorithm, 'patternsearch')
     filename = strcat(pathToFolder,'ptrnSrch_',t1,'QR.mat');

ur_idntfcn_drvgns.m

@@ -3,8 +3,8 @@ clc; clear all; close all;
 % ------------------------------------------------------------------------
 % Load data and procces it (filter and estimate accelerations)
 % ------------------------------------------------------------------------
-unloadedTrajectory = parseURData('ur-20_02_12-50sec_12harm.csv', 355, 5090);
-% unloadedTrajectory = parseURData('ur-20_01_31-unload.csv', 300, 2623);
+% unloadedTrajectory = parseURData('ur-20_02_12-50sec_12harm.csv', 355, 5090);
+unloadedTrajectory = parseURData('ur-20_01_31-unload.csv', 300, 2623);
 % unloadedTrajectory = parseURData('ur-20_02_19_14harm50sec.csv', 195, 4966);
 unloadedTrajectory = filterData(unloadedTrajectory);
 
@@ -13,7 +13,6 @@ unloadedTrajectory = filterData(unloadedTrajectory);
 loadedTrajectory = parseURData('ur-20_02_19_14harm50secLoad.csv', 308, 5071);
 loadedTrajectory = filterData(loadedTrajectory);
 
-
 % ------------------------------------------------------------------------
 % Generate Regressors based on data
 % ------------------------------------------------------------------------

ur_idntfcn_real.m

@@ -89,14 +89,13 @@ end
 
 % Set-up SDP optimization procedure
 for i = 1:length(idntfcnTrjctry)
-    [pib_SDP(:,i), pifrctn_SDP(:,i)] = physicallyConsistentEstimation(Tau{i}, Wb{i}, baseQR);
-    [pib_SDP(:,i+1), pifrctn_SDP(:,i+1)] = physicallyConsistentEstimation(Tau{i+1}, Wb{i+1}, baseQR);
+    [pib_SDP(:,i), pifrctn_SDP(:,i), pi_full] = physicallyConsistentEstimation(Tau{i}, Wb{i}, baseQR);
+    [pib_SDP(:,i+1), pifrctn_SDP(:,i+1), ~] = physicallyConsistentEstimation(Tau{i+1}, Wb{i+1}, baseQR);
 end
 
 
-return
-% 
 
+return
 %% Saving identified parameters
 pi_full = baseQR.permutationMatrix*[eye(baseQR.numberOfBaseParameters), ...
                                     -baseQR.beta; ...
@@ -110,6 +109,7 @@ identifiedUR10E.standardParameters = pi_full;
 identifiedUR10E.linearFrictionParameters = pi_frctn;
 
 
+% 
 
 %% Statisitical analysis
 % unbiased estimation of the standard deviation
@@ -158,7 +158,7 @@ end
 
 
 
-function [pib_SDP, pifrctn_SDP] = physicallyConsistentEstimation(Tau, Wb, baseQR)
+function [pib_SDP, pifrctn_SDP, pi_full] = physicallyConsistentEstimation(Tau, Wb, baseQR)
 
 physicalConsistency = 1;
 
@@ -225,4 +225,9 @@ sol2 = optimize(cnstr, obj, sdpsettings('solver','sdpt3'));
 pib_SDP = value(pi_b); % variables for base paramters
 pifrctn_SDP = value(pi_frctn);
 
+pi_full = baseQR.permutationMatrix*[eye(baseQR.numberOfBaseParameters), ...
+                                    -baseQR.beta; ...
+                                    zeros(26,baseQR.numberOfBaseParameters), ... 
+                                    eye(26) ]*[value(pi_b); value(pi_d)];
+
 end

ur_regressors_lgr.m

@@ -7,6 +7,7 @@ run('main_ur.m')
 
 generateLoadRegressorFunction = 0;
 generateFullRegressorFunction = 0;
+generateSystemMatricesFunctions = 1;
 
 % Symbolic generilized coordiates, their first and second deriatives
 q_sym = sym('q%d',[6,1],'real');
@@ -21,8 +22,6 @@ 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)';
@@ -78,36 +77,36 @@ 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)';
+beta_Ll = beta_Kl - beta_Pl;
+dbetaLl_dq = jacobian(beta_Ll,q_sym)';
+dbetaLl_dqd = jacobian(beta_Ll,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);
+   tl = tl + diff(dbetaLl_dqd,q_sym(i))*qd_sym(i)+...
+                diff(dbetaLl_dqd,qd_sym(i))*q2d_sym(i);
 end
-Y_l = tl - dLagrl_dq;
+Y_l = tl - dbetaLl_dq;
 
 if generateLoadRegressorFunction
     matlabFunction(Y_l,'File','autogen/load_regressor_UR10E',...
-                   'Vars',{q_sym,qd_sym,q2d_sym});
+                   '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_Lf = [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)';
+dbetaLf_dq = jacobian(beta_Lf,q_sym)';
+dbetaLf_dqd = jacobian(beta_Lf,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);
+   tf = tf + diff(dbetaLf_dqd,q_sym(i))*qd_sym(i)+...
+                diff(dbetaLf_dqd,qd_sym(i))*q2d_sym(i);
 end
-Y_f = tf - dLagrf_dq;
+Y_f = tf - dbetaLf_dq;
 
 if generateFullRegressorFunction
     matlabFunction(Y_f,'File','autogen/full_regressor_UR10E',...
@@ -117,10 +116,46 @@ end
 % -------------------------------------------------------------------
 % Dynamics matrices of the robot
 % -------------------------------------------------------------------
-pi_sndrd_sym = sym('pi%d%d',[60,1],'real');
-t1 = [beta_P(1,:), beta_P(2,:), beta_P(3,:), beta_P(4,:),...
-      beta_P(5,:), beta_P(6,:)];
-G_vctr_sym = jacobian(t1, q_sym)'*pi_sndrd_sym;
+if generateSystemMatricesFunctions
+
+pi_sndrd_sym = sym('pi%d%d', [60,1], 'real'); % standard parameters
+Lagr = beta_Lf*pi_sndrd_sym; % Lagrangian of the system
+P = [beta_P(1,:), beta_P(2,:), beta_P(3,:), beta_P(4,:),...
+     beta_P(5,:), beta_P(6,:)]*pi_sndrd_sym; % Potential energy
+ 
+dLagr_dqd = jacobian(Lagr, qd_sym)';
+
+M_mtrx_sym = jacobian(dLagr_dqd, qd_sym);  
+G_vctr_sym = jacobian(P, q_sym)';
+
+cs1 = sym(zeros(6,6,6)); % Christoffel symbols of the first kind
+for i = 1:1:6
+    for j = 1:1:6
+       for k = 1:1:6
+          cs1(i,j,k) = 0.5*(diff(M_mtrx_sym(i,j), q_sym(k)) + ...
+                          diff(M_mtrx_sym(i,k), q_sym(j)) - ...
+                          diff(M_mtrx_sym(j,k), q_sym(i)));
+       end
+    end
+end
+
+C_mtrx_sym = sym(zeros(6, 6));
+for i = 1:1:6
+    for j = 1:1:6
+        for k = 1:1:6
+            C_mtrx_sym(i,j) = C_mtrx_sym(i,j)+cs1(i,j,k)*qd_sym(k);
+        end
+    end
+end
+
+matlabFunction(M_mtrx_sym, 'File','autogen/M_mtrx_fcn',...
+               'Vars',{q_sym, pi_sndrd_sym}, 'Optimize', false);
+               
+matlabFunction(C_mtrx_sym, 'File','autogen/C_mtrx_fcn',...
+               'Vars',{q_sym, qd_sym, pi_sndrd_sym}, 'Optimize', false);
+
+matlabFunction(G_vctr_sym, 'File','autogen/G_vctr_fcn',...
+               'Vars',{q_sym, pi_sndrd_sym}, 'Optimize', false);
+
+end
 
-matlabFunction(G_vctr_sym, 'File','autogen/G_vctr_fcn2',...
-                   'Vars',{q_sym, pi_sndrd_sym}, 'Optimize',false);

ur_vldtn.m

@@ -2,7 +2,7 @@
 % Load validation trajectory
 % ------------------------------------------------------------------------
 close all;
-staticValidation = 1;
+staticValidation = 0;
 
 if ~staticValidation
     vldtnTrjctry = parseURData('ur-20_01_17-ptp_10_points.csv', 1, 5346);
@@ -27,13 +27,20 @@ for j = 1:length(idntfcnTrjctry)+1
     tau_OLS{j} = [];
 end
 
+t1 = reshape(pi_full, [11,6]);
+pi_full = reshape(t1(1:10,:), [60,1]);
+pi_drvs = t1(11,:)';
 for i = 1:length(vldtnTrjctry.t)
-    Yi = regressorWithMotorDynamics(vldtnTrjctry.q(i,:)',...
-                                    vldtnTrjctry.qd_fltrd(i,:)',...
-                                    vldtnTrjctry.q2d_est(i,:)');
+    qi = vldtnTrjctry.q(i,:)';
+    qdi = vldtnTrjctry.qd_fltrd(i,:)';
+    q2di = vldtnTrjctry.q2d_est(i,:)';
+    Yi = regressorWithMotorDynamics(qi, qdi, q2di);
     
     Ybi = Yi*baseQR.permutationMatrix(:,1:baseQR.numberOfBaseParameters);
-    Yfrctni = frictionRegressor(vldtnTrjctry.qd_fltrd(i,:)');
+    Yfrctni = frictionRegressor(qdi);
+    
+    tau1 = M_mtrx_fcn(qi, pi_full)*q2di + C_mtrx_fcn(qi, qdi, pi_full)*qdi + G_vctr_fcn(qi, pi_full)
+    tau2 = Ybi*pib_SDP(:,1)
     
     tau_msrd = horzcat(tau_msrd, diag(drvGains)*vldtnTrjctry.i(i,:)');
     
@@ -44,16 +51,18 @@ for i = 1:length(vldtnTrjctry.t)
         tau_OLS{j} = horzcat(tau_OLS{j}, [Ybi Yfrctni]*[pib_OLS(:,j); pifrctn_OLS(:,j)]);
     end
     i_SDP{j+1} = horzcat(i_SDP{j+1}, diag(drvGains2)\([Ybi Yfrctni]*[pib_SDP(:,j+1); pifrctn_SDP(:,j+1)]));
+    
 end
 
 %%
+clrs = {'r', 'b'};
 
 for i = 1:6
     figure
     hold on
     plot(vldtnTrjctry.t, vldtnTrjctry.i(:,i), 'k-')
     for j = 1:length(idntfcnTrjctry)+1
-        plot(vldtnTrjctry.t, i_SDP{j}(i,:), 'LineWidth',1.5)
+        plot(vldtnTrjctry.t, i_SDP{j}(i,:), clrs{j}, 'LineWidth',1.5)
     end
     ylabel('\tau, Nm')
     xlabel('t, sec')

ur_vldtn.m~

@@ -1,71 +0,0 @@
-% ------------------------------------------------------------------------
-% Load validation trajectory
-% ------------------------------------------------------------------------
-close all;
-staticValidation = 0;
-
-if ~staticValidation
-% vldtnTrjctry = parseURData('ur-19_10_01-14_04_13.csv', 1, 759);
-% vldtnTrjctry = parseURData('ur-19_10_01-13_51_41.csv', 1, 660);
-% vldtnTrjctry = parseURData('ur-19_09_27-11_28_22.csv', 1, 594);
-%     vldtnTrjctry = parseURData('ur-20_01_17-ptp_10_points.csv', 1, 5346);
-% vldtnTrjctry = parseURData('ur-19_12_23_free.csv', 1, 2005);
-    vldtnTrjctry = filterData(vldtnTrjctry);
-else
-    load('vldtnTrjctrySttcs.mat');
-    vldtnTrjctry = vldtnTrjctrySttcs;
-end
-
-% -----------------------------------------------------------------------
-% Predicting torques
-% -----------------------------------------------------------------------
-%Constracting regressor matrix
-tau_msrd = []; 
-i_OLS = {}; i_SDP = {};
-tau_OLS = {}; tau_SDP = {};
-for j = 1:length(idntfcnTrjctry)+1
-    i_OLS{j} = [];
-    i_SDP{j} = [];
-    tau_SDP{j} = [];
-    tau_OLS{j} = [];
-end
-
-for i = 1:length(vldtnTrjctry.t)
-    Yi = regressorWithMotorDynamics(vldtnTrjctry.q(i,:)',...
-                                    vldtnTrjctry.qd_fltrd(i,:)',...
-                                    vldtnTrjctry.q2d_est(i,:)');
-    
-    Ybi = Yi*baseQR.permutationMatrix(:,1:baseQR.numberOfBaseParameters);
-    Yfrctni = frictionRegressor(vldtnTrjctry.qd_fltrd(i,:)');
-    
-    tau_msrd = horzcat(tau_msrd, diag(drvGains)*vldtnTrjctry.i(i,:)');
-    
-    for j = 1:length(idntfcnTrjctry)
-        i_OLS{j} = horzcat(i_OLS{j}, diag(drvGains)\([Ybi Yfrctni]*[pib_OLS(:,j); pifrctn_OLS(:,j)]));
-        i_SDP{j} = horzcat(i_SDP{j}, diag(drvGains)\([Ybi Yfrctni]*[pib_SDP(:,j); pifrctn_SDP(:,j)]));
-        tau_SDP{j} = horzcat(tau_SDP{j}, [Ybi Yfrctni]*[pib_SDP(:,j); pifrctn_SDP(:,j)]);
-        tau_OLS{j} = horzcat(tau_OLS{j}, [Ybi Yfrctni]*[pib_OLS(:,j); pifrctn_OLS(:,j)]);
-    end
-    i_SDP{j+1} = horzcat(i_SDP{j+1}, diag(drvGains2)\([Ybi Yfrctni]*[pib_SDP(:,j+1); pifrctn_SDP(:,j+1)]));
-end
-
-%%
-
-for i = 1:6
-    figure
-    hold on
-    plot(vldtnTrjctry.t, vldtnTrjctry.i(:,i), 'k-')
-    for j = 1:length(idntfcnTrjctry)+1
-        plot(vldtnTrjctry.t, i_SDP{j}(i,:), 'LineWidth',1.5)
-    end
-    ylabel('\tau, Nm')
-    xlabel('t, sec')
-    grid on
-end
-
-
-
-
-
-
-