diff --git a/trajectory_optmzn/ptrnSrch_N5T20.mat b/trajectory_optmzn/ptrnSrch_N5T20.mat
new file mode 100644
index 0000000..82df1f6
Binary files /dev/null and b/trajectory_optmzn/ptrnSrch_N5T20.mat differ
diff --git a/ur_base_params_QRlgr.m b/ur_base_params_QRlgr.m
index b5aa4ea..b8e0378 100644
--- a/ur_base_params_QRlgr.m
+++ b/ur_base_params_QRlgr.m
@@ -129,9 +129,10 @@ wr = invG*we;
 % -----------------------------------------------------------------------
 fprintf('Validation of mapping from standard parameters to base ones\n')
 if includeMotorDynamics
-    ur10.pi = rand(nLnkPrms*nLnks, 1);
+    ur10.pi(end+1,:) = rand(1,nLnks);
+    ur10.pi = reshape(ur10.pi,[nLnkPrms*nLnks, 1]);
 else
-    ur10.pi = reshape(ur10.pi,[60,1]);
+    ur10.pi = reshape(ur10.pi,[nLnkPrms*nLnks, 1]);
 end
 
 % On random positions, velocities, aceeleations
diff --git a/ur_base_params_QRlgr.m~ b/ur_base_params_QRlgr.m~
deleted file mode 100644
index cf8281f..0000000
--- a/ur_base_params_QRlgr.m~
+++ /dev/null
@@ -1,175 +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;
-%}
-
-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)<sqrt(eps)) = sym(0); % to avoid numerical errors
-p_6ee = str2num(ur10.robot.joint{7}.origin.Attributes.xyz)';
-T_6ee = sym([R_6ee, p_6ee; zeros(1,3), 1]); % to avoid numerical errors
 
-w_eeee = T_6ee(1:3,1:3)'*w_kk(:,7);
-v_eeee = T_6ee(1:3,1:3)'*(v_kk(:,7) + cross(w_kk(:,i+1),sym(p_6ee)));
-g_eeee = T_6ee(1:3,1:3)'*g_kk(:,7);
-p_eeee = T_6ee(1:3,1:3)'*(p_kk(:,7) + sym(p_6ee));
-
-K_l = [sym(0.5)*w2wtlda(w_eeee), v_eeee'*vec2skewSymMat(w_eeee),...
-            sym(0.5)*(v_eeee'*v_eeee)];
-        
-P_l = [sym(zeros(1,6)), g_eeee', g_eeee'*p_eeee];
-
-% -----------------------------------------------------
-% Dynamic regressor of the load
-% -----------------------------------------------------
-Lagrl = K_l - P_l;
-dLagrl_dq = jacobian(Lagrl,q_sym)';
-dLagrl_dqd = jacobian(Lagrl,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);
-end
-Y_l = tl - dLagrl_dq;
-% matlabFunction(Y_l,'File','idntfcn/load_regressor_UR10E',...
-%                 'Vars',{q_sym,qd_sym,q2d_sym});
-
-% ----------------------------------------------------
-% Dynamic regressor of the full paramters
-% ----------------------------------------------------
-Lagrf = [K_reg(1,:) - P_reg(1,:), K_reg(2,:) - P_reg(2,:),...
-            K_reg(3,:) - P_reg(3,:), K_reg(4,:) - P_reg(4,:),...
-            K_reg(5,:) - P_reg(5,:), K_reg(6,:) - P_reg(6,:)];
-dLagrf_dq = jacobian(Lagrf,q_sym)';
-dLagrf_dqd = jacobian(Lagrf,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);
-end
-Y_f = tf - dLagrf_dq;
-matlabFunction(Y_f,'File','autogen/full_regressor_UR10E',...
-                'Vars',{q_sym,qd_sym,q2d_sym});
-
-return
 % -----------------------------------------------------------------------
 % Regrouping parameters of the links
 % -----------------------------------------------------------------------
@@ -160,6 +101,7 @@ for i = 6:-1:2
 end
 pir_ur10_num = double(pir_ur10_num);
 
+
 % -----------------------------------------------------------------------
 % Getting base parameters
 % -----------------------------------------------------------------------
@@ -209,20 +151,23 @@ for i = 1:6
     pir_base_vctr = vertcat(pir_base_vctr,pir_base_num{i});
 end
 
+
 % -----------------------------------------------------------------------
 % Computing Regressor
 % -----------------------------------------------------------------------
 dLagr_dq = jacobian(grad_Lagr,q_sym)';
 dLagr_dqd = jacobian(grad_Lagr,qd_sym)';
 t1 = sym(zeros(6,length(pir_base_vctr)));
-% t1 = sym(zeros(6,42));
 for i = 1:6
    t1 = t1 + diff(dLagr_dqd,q_sym(i))*qd_sym(i)+...
                 diff(dLagr_dqd,qd_sym(i))*q2d_sym(i);
 end
 Y_hat = t1 - dLagr_dq;
-% matlabFunction(Y_hat,'File','idntfcn/base_regressor_UR10E',...
-%                 'Vars',{q_sym,qd_sym,q2d_sym});
+
+if generateBaseRegressorFunction
+    matlabFunction(Y_hat,'File','autogen/base_regressor_UR10E',...
+                   'Vars',{q_sym,qd_sym,q2d_sym});
+end
 
 % ------------------------------------------------------------
 % Finding mapping from standard parameters to base parameters
@@ -234,6 +179,7 @@ pi_ur10_base = [pir_base_sym{1};pir_base_sym{2};pir_base_sym{3};...
 dbase_dfull = jacobian(pi_ur10_base,pi_ur10_full);
 Kd = jacobian(pi_ur10_base - Pb*pi_ur10_full, Pd*pi_ur10_full);
 
+
 % --------------------------------------------------------------------
 % Computing matrices of dynamic equations of motion 
 % --------------------------------------------------------------------
@@ -277,18 +223,19 @@ for i = 1:1:6
     end
 end
 
-% Generating matrices
-fprintf('Generating dynamic equatio elements:\n')
-%{
-fprintf('\t Mass matrix\n')
-matlabFunction(M_mtrx_sym,'File','autogen/M_mtrx_fcn','Vars',{q_sym,xi});
+if generateBaseDynamicsFunctions
+    fprintf('Generating dynamic equation elements:\n');
 
-fprintf('\t Matrix of Coriolis and Centrifugal Forces\n')
-matlabFunction(C_mtrx_sym,'File','autogen/C_mtrx_fcn','Vars',{q_sym,qd_sym,xi});
+    fprintf('\t Mass matrix\n');
+    matlabFunction(M_mtrx_sym,'File','autogen/M_mtrx_fcn','Vars',{q_sym,xi});
+
+    fprintf('\t Matrix of Coriolis and Centrifugal Forces\n');
+    matlabFunction(C_mtrx_sym,'File','autogen/C_mtrx_fcn','Vars',{q_sym,qd_sym,xi});
+
+    fprintf('\t Vector of gravitational forces\n');
+    matlabFunction(G_vctr_sym,'File','autogen/G_vctr_fcn','Vars',{q_sym,xi});
+end
 
-fprintf('\t Vector of gravitational forces\n')
-matlabFunction(G_vctr_sym,'File','autogen/G_vctr_fcn','Vars',{q_sym,xi});
-%}
 return
 % --------------------------------------------------------------------
 % Tests
diff --git a/ur_exprmt_dsgn.m b/ur_exprmt_dsgn.m
index 34959d1..1c926fa 100644
--- a/ur_exprmt_dsgn.m
+++ b/ur_exprmt_dsgn.m
@@ -4,6 +4,9 @@
 % ---------------------------------------------------------------------
 run('main_ur.m'); % get robot description
 
+% Choose optimization algorithm: 'patternsearch', 'ga'
+optmznAlgorithm = 'patternsearch';
+
 % Getting limits on posistion and velocities. Moreover we get some
 % constant parmeters of the robot that allow us to accelerate computation
 % of the robot regressor.
@@ -38,52 +41,73 @@ traj_par.t = 0:traj_par.t_smp:traj_par.T;  % time
 traj_par.N = 5;          % number of harmonics
 
 
-%  ------------------------------------------------------------------------
+%  ----------------------------------------------------------------------
 % Otimization
-% ------------------------------------------------------------------------
+% -----------------------------------------------------------------------
 A = []; b = [];
 Aeq = []; beq = [];
 lb = []; ub = [];
-x0 = rand(6*2*traj_par.N,1);
 
-optns_pttrnSrch = optimoptions('patternsearch');
-optns_pttrnSrch.Display = 'iter';
+if strcmp(optmznAlgorithm, 'patternsearch')
+    x0 = rand(6*2*traj_par.N,1);
+    optns_pttrnSrch = optimoptions('patternsearch');
+    optns_pttrnSrch.Display = 'iter';
+    optns_pttrnSrch.StepTolerance = 1e-3;
 
-x = patternsearch(@(x)traj_cost_lgr(x,traj_par,ur10),x0,A,b,Aeq,beq,lb,ub,...
-                  @(x)traj_cnstr(x,traj_par,ur10), optns_pttrnSrch);
-
-
-return
-
-options = optimoptions('ga');
-options.Display = 'iter';
-options.PlotFcn = 'gaplotbestf'; % {'gaplotbestf', 'gaplotscores'}
-options.MaxGenerations = 50;
-options.PopulationSize = 1e+3; % in each generation.
-options.InitialPopulationRange = [-100; 100];
-options.SelectionFcn = 'selectionroulette';
-
-[x,fval,exitflag,output,population,scores] = ga(@(x)traj_cost(x,traj_par,ur10),...
-        6*2*traj_par.N,A,b,Aeq,beq,lb,ub,@(x)traj_cnstr(x,traj_par,ur10),options);
+    [x,fval] = patternsearch(@(x)traj_cost_lgr(x,traj_par,ur10), x0, ...
+                             A, b, Aeq, beq, lb, ub, ...
+                             @(x)traj_cnstr(x,traj_par,ur10), optns_pttrnSrch);
+              
+elseif strcmp(optmznAlgorithm, 'ga')
+    optns_ga = optimoptions('ga');
+    optns_ga.Display = 'iter';
+    optns_ga.PlotFcn = 'gaplotbestf'; % {'gaplotbestf', 'gaplotscores'}
+    optns_ga.MaxGenerations = 50;
+    optns_ga.PopulationSize = 1e+3; % in each generation.
+    optns_ga.InitialPopulationRange = [-100; 100];
+    optns_ga.SelectionFcn = 'selectionroulette';
 
+    [x,fval] = ga(@(x)traj_cost_lgr(x,traj_par,ur10), 6*2*traj_par.N,...
+                  A, b, Aeq, beq, lb, ub, ...
+                  @(x)traj_cnstr(x,traj_par,ur10),optns_ga);
+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 = pol_coeffs(traj_par.T,a,b,traj_par.wf,traj_par.N); % polynomial coeffs
-[q,qd,q2d] = mixed_traj(traj_par.t,c_pol,a,b,traj_par.wf,traj_par.N);
-    
-traj_cnstr(x,traj_par,ur10)
+c_pol = getPolCoeffs(traj_par.T, a, b, traj_par.wf, traj_par.N, ur10.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)
+    ylabel('$q$','interpreter','latex')
+    grid on
     legend('q1','q2','q3','q4','q5','q6')
 subplot(3,1,2)
     plot(traj_par.t,qd)
+    ylabel('$\dot{q}$','interpreter','latex')
+    grid on
     legend('qd1','qd2','qd3','qd4','qd5','qd6')
 subplot(3,1,3)
     plot(traj_par.t,q2d)
-    legend('q2d1','q2d2','q2d3','q2d4','q2d5','q2d6')
\ No newline at end of file
+    ylabel('$\ddot{q}$','interpreter','latex')
+    grid on
+    legend('q2d1','q2d2','q2d3','q2d4','q2d5','q2d6')
+
+    
+pathToFolder = 'trajectory_optmzn/';
+if strcmp(optmznAlgorithm, 'patternsearch')
+    t1 = strcat('N',num2str(traj_par.N),'T',num2str(traj_par.T));
+    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');
+end
+save(filename,'a','b','c_pol','traj_par')
diff --git a/ur_exprmt_dsgn.m~ b/ur_exprmt_dsgn.m~
new file mode 100644
index 0000000..0104edf
--- /dev/null
+++ b/ur_exprmt_dsgn.m~
@@ -0,0 +1,108 @@
+% ---------------------------------------------------------------------
+% In this script trajectory optimization otherwise called experiment
+% design for dynamic paramters identification is carried out. 
+% ---------------------------------------------------------------------
+run('main_ur.m'); % get robot description
+
+% Choose optimization algorithm: 'patternsearch', 'ga'
+optmznAlgorithm = 'patternsearch';
+
+% Getting limits on posistion and velocities. Moreover we get some
+% constant parmeters of the robot that allow us to accelerate computation
+% of the robot regressor.
+q_min = zeros(6,1); q_max = zeros(6,1); qd_max = zeros(6,1);
+for i = 1:6
+    rot_axes(:,i) = str2num(ur10.robot.joint{i}.axis.Attributes.xyz)';
+    R_pj = RPY(str2num(ur10.robot.joint{i}.origin.Attributes.rpy));
+    p_pj = str2num(ur10.robot.joint{i}.origin.Attributes.xyz)';
+    T_pj(:,:,i) = [R_pj, p_pj; zeros(1,3), 1];
+    r_com(:,i) = str2num(ur10.robot.link{i+1}.inertial.origin.Attributes.xyz)';
+    
+    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
+
+ur10.rot_axes = rot_axes; % axis of rotation of each joint
+ur10.T_pj = T_pj;
+ur10.r_com = r_com;
+ur10.qd_max = qd_max;
+ur10.q2d_max = 2*ones(6,1);
+% Use different limit for positions for safety
+ur10.q_min = deg2rad([-180 -180 -90 -180 -90 -90]');
+ur10.q_max = deg2rad([180 0 90 0 90 90]');
+ur10.q0 = deg2rad([0 -90 0 -90 0 0 ]');
+
+% Trajectory parameters
+traj_par.T = 20;          % period of signal
+traj_par.wf = 2*pi/traj_par.T;    % fundamental frequency
+traj_par.t_smp = 1e-1;   % sampling time
+traj_par.t = 0:traj_par.t_smp:traj_par.T;  % time
+traj_par.N = 5;          % number of harmonics
+
+
+%  ----------------------------------------------------------------------
+% Otimization
+% -----------------------------------------------------------------------
+A = []; b = [];
+Aeq = []; beq = [];
+lb = []; ub = [];
+
+if strcmp(optmznAlgorithm, 'patternsearch')
+    x0 = rand(6*2*traj_par.N,1);
+    optns_pttrnSrch = optimoptions('patternsearch');
+    optns_pttrnSrch.Display = 'iter';
+    optns_pttrnSrch.StepTolerance = 1e-3;
+
+    [x,fval] = patternsearch(@(x)traj_cost_lgr(x,traj_par,ur10), x0, ...
+                             A, b, Aeq, beq, lb, ub, ...
+                             @(x)traj_cnstr(x,traj_par,ur10), optns_pttrnSrch);
+              
+elseif strcmp(optmznAlgorithm, 'ga')
+    optns_ga = optimoptions('ga');
+    optns_ga.Display = 'iter';
+    optns_ga.PlotFcn = 'gaplotbestf'; % {'gaplotbestf', 'gaplotscores'}
+    optns_ga.MaxGenerations = 50;
+    optns_ga.PopulationSize = 1e+3; % in each generation.
+    optns_ga.InitialPopulationRange = [-100; 100];
+    optns_ga.SelectionFcn = 'selectionroulette';
+
+    [x,fval] = ga(@(x)traj_cost_lgr(x,traj_par,ur10), 6*2*traj_par.N,...
+                  A, b, Aeq, beq, lb, ub, ...
+                  @(x)traj_cnstr(x,traj_par,ur10),optns_ga);
+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);
+[q,qd,q2d] = mixed_traj(traj_par.t, c_pol, a, b, traj_par.wf, traj_par.N);
+
+
+if strcmp(optmznAlgorithm, 'patternsearch')
+    strcat('N',num2str(traj_par.N),'T',num2str(traj_par.T))
+elseif strcmp(optmznAlgorithm, 'ga')
+    
+end
+
+figure
+subplot(3,1,1)
+    plot(traj_par.t,q)
+    ylabel('$q$','interpreter','latex')
+    grid on
+    legend('q1','q2','q3','q4','q5','q6')
+subplot(3,1,2)
+    plot(traj_par.t,qd)
+    ylabel('$\dot{q}$','interpreter','latex')
+    grid on
+    legend('qd1','qd2','qd3','qd4','qd5','qd6')
+subplot(3,1,3)
+    plot(traj_par.t,q2d)
+    ylabel('$\ddot{q}$','interpreter','latex')
+    grid on
+    legend('q2d1','q2d2','q2d3','q2d4','q2d5','q2d6')
\ No newline at end of file
diff --git a/ur_regressors_lgr.m b/ur_regressors_lgr.m
new file mode 100644
index 0000000..c62bc95
--- /dev/null
+++ b/ur_regressors_lgr.m
@@ -0,0 +1,115 @@
+% ----------------------------------------------------------------------
+% In this file full regressor of the robot as well as load regressor
+% are obtained using Lagrange formulation of dynamics.
+% ----------------------------------------------------------------------
+% Get robot description
+run('main_ur.m')
+
+generateLoadRegressorFunction = 0;
+generateFullRegressorFunction = 0;
+
+% 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');
+
+% ------------------------------------------------------------------------
+% Getting gradient of energy functions, to derive dynamics
+% ------------------------------------------------------------------------
+T_pk = sym(zeros(4,4,6)); % transformation between links
+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)';
+% Transformation from parent link frame p to current joint frame
+    rpy_k = sym(str2num(ur10.robot.joint{i}.origin.Attributes.rpy));
+    R_pj = RPY(rpy_k);
+    R_pj(abs(R_pj)<sqrt(eps)) = sym(0); % to avoid numerical errors
+    p_pj = str2num(ur10.robot.joint{i}.origin.Attributes.xyz)';
+    T_pj = sym([R_pj, p_pj; zeros(1,3), 1]); % to avoid numerical errors
+% Tranformation from joint frame of the joint that rotaties body k to
+% link frame. The transformation is pure rotation
+    R_jk = Rot(q_sym(i),sym(jnt_axs_k));
+    p_jk = sym(zeros(3,1));
+    T_jk = [R_jk, p_jk; sym(zeros(1,3)),sym(1)];
+% Transformation from parent link frame p to current link frame k
+    T_pk(:,:,i) = T_pj*T_jk;
+    z_kk(:,i) = sym(jnt_axs_k);
+        
+    w_kk(:,i+1) = T_pk(1:3,1:3,i)'*w_kk(:,i) + sym(jnt_axs_k)*qd_sym(i);
+    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));
+        
+    beta_K(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)];
+    beta_P(i,:) = [sym(zeros(1,6)), g_kk(:,i+1)',...
+                   g_kk(:,i+1)'*p_kk(:,i+1)];
+end
+
+
+% --------------------------------------------------------------------
+% Gradient of the 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)<sqrt(eps)) = sym(0); % to avoid numerical errors
+p_6ee = str2num(ur10.robot.joint{7}.origin.Attributes.xyz)';
+T_6ee = sym([R_6ee, p_6ee; zeros(1,3), 1]); % to avoid numerical errors
+
+w_eeee = T_6ee(1:3,1:3)'*w_kk(:,7);
+v_eeee = T_6ee(1:3,1:3)'*(v_kk(:,7) + cross(w_kk(:,i+1),sym(p_6ee)));
+g_eeee = T_6ee(1:3,1:3)'*g_kk(:,7);
+p_eeee = T_6ee(1:3,1:3)'*(p_kk(:,7) + sym(p_6ee));
+
+beta_Kl = [sym(0.5)*w2wtlda(w_eeee), v_eeee'*vec2skewSymMat(w_eeee),...
+            sym(0.5)*(v_eeee'*v_eeee)];
+        
+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)';
+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);
+end
+Y_l = tl - dLagrl_dq;
+
+if generateLoadRegressorFunction
+    matlabFunction(Y_l,'File','autogen/load_regressor_UR10E',...
+                   '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_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)';
+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);
+end
+Y_f = tf - dLagrf_dq;
+
+if generateFullRegressorFunction
+    matlabFunction(Y_f,'File','autogen/full_regressor_UR10E',...
+                   'Vars',{q_sym,qd_sym,q2d_sym});
+end
\ No newline at end of file