need to check why Wb is not good

Identification_main.m

@@ -30,5 +30,6 @@ robot = get_baseParams(robot, opt);
 % readDataFile;
 % robot.posData = posData;
 % robot.currentData = currentData;
-% % % robot = estimate_dyn(robot,opt);
+% robot = estimate_dyn(robot,opt);
-% robot = estimate_dyn_form_data(robot,opt);
+% robot = estimate_dyn_form_data(robot,opt);
+robot = estimate_dyn_MLS(robot,opt);

codegen/base_regressor_R1000_EVT.m

@@ -1,4 +1,4 @@
-function base_regrssor  = base_regressor_R1000_DVT(theta,dtheta,ddtheta,baseQR)
+function base_regrssor  = base_regressor_R1000_EVT(theta,dtheta,ddtheta,baseQR)
 opt.robot_def = 'direct';
 opt.KM_method = 'SCREW';
 opt.Vel_method = 'Direct';
@@ -12,7 +12,7 @@ opt.isSixAxisFTSensor = true;
 file=[];
 
 robot = get_robot_R1000_EVT(theta,zeros(size(dtheta)),zeros(size(ddtheta)),file,opt);
-robot = get_Kinematics_EVT(robot, opt);
+robot = get_Kinematics_R1000_EVT(robot, opt);
 robot = get_velocity(robot, opt);
 robot = get_regressor(robot, opt);
 % get base params

estimate_dyn_MLS.m

@@ -0,0 +1,110 @@
+function robot = estimate_dyn_MLS(robot,opt)
+% -------------------------------------------------------------------
+% Get datas
+% ------------------------------------------------------------------------
+get_GCTraj_R1000_EVT;
+% -------------------------------------------------------------------
+% Generate Regressors based on data
+% ------------------------------------------------------------------------
+drvGains = [];
+baseQR = robot.baseQR;
+
+for i = 1:1:robot.ndof
+    q = idntfcnTrjctry(i).q;
+    qd = idntfcnTrjctry(i).qd;
+    qdd = idntfcnTrjctry(i).qdd;
+    [nRow,nCol] = size(idntfcnTrjctry(i).qd);
+    Wb = []; Tau = [];
+    for j = 1:nRow/robot.ndof
+        for k = 1:nCol
+            if opt.isJointTorqueSensor
+                base_regressor_func = sprintf('base_regressor_%s',opt.robotName);
+                Yb = feval(base_regressor_func, q(robot.ndof*(j-1)+1:robot.ndof*j,k),...
+                    qd(robot.ndof*(j-1)+1:robot.ndof*j,k),...
+                    qdd(robot.ndof*(j-1)+1:robot.ndof*j,k),baseQR);
+                Yb = Yb(i,:);
+                Wb = vertcat(Wb, Yb);
+                Tau = vertcat(Tau, idntfcnTrjctry(i).tau(j,k));
+            end
+        end
+    end
+    observationMatrix(i).Wb = Wb;
+    observationMatrix(i).Tau = Tau;
+    observationMatrix(i).rank = robot.baseQR.rank(i);
+end
+
+% ---------------------------------------------------------------------
+% Estimate parameters
+% ---------------------------------------------------------------------
+sol = struct;
+for i = 9:-1:1
+    Wb = observationMatrix(i).Wb;
+    Tau = observationMatrix(i).Tau;
+%     [nRow,nCol] = size(Wb);
+    if i == 9
+        pib_OLS=pinv(Wb(:,1:observationMatrix(i).rank))*Tau;
+        pib_MLS = [];
+    elseif i > 1
+        pib_OLS=pinv(Wb(:,observationMatrix(i+1).rank+1:observationMatrix(i).rank))*...
+            (-Wb(:,1:observationMatrix(i+1).rank)*pib_MLS+Tau);
+    else
+        break;
+    end
+    pifrctn_OLS = 0;
+    pib_MLS = [pib_MLS;pib_OLS];
+end
+a=1
+% method = 'OLS';
+% if strcmp(method, 'OLS')
+%     % Usual least squares
+%     [sol.pi_b, sol.pi_fr] = ordinaryLeastSquareEstimation(observationMatrix);
+% elseif strcmp(method, 'PC-OLS')
+%     % Physically consistent OLS using SDP optimization
+%     [sol.pi_b, sol.pi_fr, sol.pi_s] = physicallyConsistentEstimation(Tau, Wb, baseQR);
+% else
+%     error("Chosen method for dynamic parameter estimation does not exist");
+% end
+% robot.sol = sol;
+% % Local unctions
+%     function observationMatrix = buildObservationMatrices(idntfcnTrjctry, baseQR, drvGains,opt)
+%         for i = 1:1:robot.ndof
+%             [nRow,nCol] = size(idntfcnTrjctry(i).qd);
+%             Wb = []; Tau = [];
+%             for j = 1:nRow/robot.ndof
+%                 for k = 1:nCol
+%                     if opt.isJointTorqueSensor
+%                         base_regressor_func = sprintf('base_regressor_%s',opt.robotName);
+%                         Yb = feval(base_regressor_func, q(robot.ndof*(j-1)+1:robot.ndof*j,k),...
+%                             qd(robot.ndof*(j-1)+1:robot.ndof*j,k),...
+%                             qdd(robot.ndof*(j-1)+1:robot.ndof*j,k),baseQR);
+%                         Yb = Yb(i,:);
+%                         Wb = vertcat(Wb, Yb);
+%                         Tau = vertcat(Tau, idntfcnTrjctry(i).tau(j,k));
+%                     end
+%                 end
+%             end
+%             observationMatrix(i).Wb = Wb;
+%             observationMatrix(i).Tau = Tau;
+%         end
+%     end
+% 
+% 
+%     function [pib_OLS, pifrctn_OLS] = ordinaryLeastSquareEstimation(observationMatrix)
+%         % Function perfroms ordinary least squares estimation of parameters
+% %         pi_OLS = (Wb'*Wb)\(Wb'*Tau);
+% %         pib_OLS = pi_OLS(1:40); % variables for base paramters
+% %         pifrctn_OLS = pi_OLS(41:end);
+%         for i = 9:-1:1
+%           Wb = observationMatrix(i).Wb;
+%           Tau = observationMatrix(i).Tau;
+%           pib_OLS(i)=pinv(Wb)*Tau;
+%           pifrctn_OLS = 0;
+%         end
+%     end
+%     function [pib_OLS, pifrctn_OLS] = MultiLeastSquareEstimation(idntfcnTrjctry, Tau, Wb)
+%         % Function perfroms Multi step ordinary least squares estimation of parameters
+%           
+%         pib_OLS=pinv(Wb)*Tau;
+%         pifrctn_OLS = 0;
+%     end
+end

get_GCTraj_R1000_EVT.m

@@ -0,0 +1,219 @@
+% function robot = get_GCTraj_R1000_EVT(robot,opt)
+% J9 traj
+time = 0:0.1:1;
+f=1;
+q_J = sin(2*pi*f*time);
+q=zeros(robot.ndof,length(q_J));
+q(9,:)=q_J;
+q(8,:)=pi/3*ones(size(q_J));
+qd=zeros(robot.ndof,length(q_J));qdd=qd;
+idntfcnTrjctry(9).t = time;
+idntfcnTrjctry(9).q = q;
+idntfcnTrjctry(9).qd = qd;
+idntfcnTrjctry(9).qdd = qdd;
+
+time = 0:0.1:1;
+f=1;
+q_J = sin(2*pi*f*time);
+q=zeros(robot.ndof,length(q_J));
+q(9,:)=q_J;
+q(8,:)=-pi/3*ones(size(q_J));
+qd=zeros(robot.ndof,length(q_J));qdd=qd;
+idntfcnTrjctry(9).t = vertcat(idntfcnTrjctry(9).t,time);
+idntfcnTrjctry(9).q = vertcat(idntfcnTrjctry(9).q,q);
+idntfcnTrjctry(9).qd = vertcat(idntfcnTrjctry(9).qd,qd);
+idntfcnTrjctry(9).qdd = vertcat(idntfcnTrjctry(9).qdd,qdd);
+
+time = 0:0.1:1;
+f=1;
+q_J = sin(2*pi*f*time);
+q=zeros(robot.ndof,length(q_J));
+q(9,:)=q_J;
+q(6,:)=pi/2*ones(size(q_J));
+qd=zeros(robot.ndof,length(q_J));qdd=qd;
+idntfcnTrjctry(9).t = vertcat(idntfcnTrjctry(9).t,time);
+idntfcnTrjctry(9).q = vertcat(idntfcnTrjctry(9).q,q);
+idntfcnTrjctry(9).qd = vertcat(idntfcnTrjctry(9).qd,qd);
+idntfcnTrjctry(9).qdd = vertcat(idntfcnTrjctry(9).qdd,qdd);
+
+time = 0:0.1:1;
+f=1;
+q_J = sin(2*pi*f*time);
+q=zeros(robot.ndof,length(q_J));
+q(9,:)=q_J;
+q(6,:)=-pi/2*ones(size(q_J));
+qd=zeros(robot.ndof,length(q_J));qdd=qd;
+idntfcnTrjctry(9).t = vertcat(idntfcnTrjctry(9).t,time);
+idntfcnTrjctry(9).q = vertcat(idntfcnTrjctry(9).q,q);
+idntfcnTrjctry(9).qd = vertcat(idntfcnTrjctry(9).qd,qd);
+idntfcnTrjctry(9).qdd = vertcat(idntfcnTrjctry(9).qdd,qdd);
+
+% J8 traj
+time = 0:0.1:1;
+f=1;
+q_J = sin(2*pi*f*time);
+q=zeros(robot.ndof,length(q_J));
+q(8,:)=pi/3*q_J;
+q(6,:)=pi/2*ones(size(q_J));
+qd=zeros(robot.ndof,length(q_J));qdd=qd;
+idntfcnTrjctry(8).t = time;
+idntfcnTrjctry(8).q = q;
+idntfcnTrjctry(8).qd = qd;
+idntfcnTrjctry(8).qdd = qdd;
+
+time = 0:0.1:1;
+f=1;
+q_J = sin(2*pi*f*time);
+q=zeros(robot.ndof,length(q_J));
+q(8,:)=pi/3*q_J;
+q(6,:)=-pi/2*ones(size(q_J));
+qd=zeros(robot.ndof,length(q_J));qdd=qd;
+idntfcnTrjctry(8).t = vertcat(idntfcnTrjctry(8).t,time);
+idntfcnTrjctry(8).q = vertcat(idntfcnTrjctry(8).q,q);
+idntfcnTrjctry(8).qd = vertcat(idntfcnTrjctry(8).qd,qd);
+idntfcnTrjctry(8).qdd = vertcat(idntfcnTrjctry(8).qdd,qdd);
+
+% J7 traj
+time = 0:0.1:1;
+f=1;
+q_J = sin(2*pi*f*time);
+% q(7,:)=q_J;
+% q(6,:)=q_J;q(5,:)=q_J;q(8,:)=q_J;
+% q(4,:)=q_J;q(9,:)=q_J;
+qd=zeros(robot.ndof,length(q_J));qdd=qd;
+q=[q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J];
+idntfcnTrjctry(7).t = time;
+idntfcnTrjctry(7).q = q;
+idntfcnTrjctry(7).qd = qd;
+idntfcnTrjctry(7).qdd = qdd;
+
+time = 0:0.1:1;
+f=1;
+q_J = sin(2*pi*f*time);
+% q(7,:)=q_J;
+% q(6,:)=q_J;q(5,:)=q_J;q(8,:)=q_J;
+% q(4,:)=q_J;q(9,:)=q_J;
+qd=zeros(robot.ndof,length(q_J));qdd=qd;
+q=[q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J];
+idntfcnTrjctry(7).t = vertcat(idntfcnTrjctry(7).t,time);
+idntfcnTrjctry(7).q = vertcat(idntfcnTrjctry(7).q,q);
+idntfcnTrjctry(7).qd = vertcat(idntfcnTrjctry(7).qd,qd);
+idntfcnTrjctry(7).qdd = vertcat(idntfcnTrjctry(7).qdd,qdd);
+
+% J6 traj
+time = 0:0.1:1;
+f=1;
+q_J = sin(2*pi*f*time);
+q=zeros(robot.ndof,length(q_J));
+q(6,:)=pi/2*q_J;
+q(4,:)=pi/4*ones(size(q_J));
+qd=zeros(robot.ndof,length(q_J));qdd=qd;
+idntfcnTrjctry(6).t= time;
+idntfcnTrjctry(6).q= q;
+idntfcnTrjctry(6).qd= qd;
+idntfcnTrjctry(6).qdd= qdd;
+
+time = 0:0.1:1;
+f=1;
+q_J = sin(2*pi*f*time);
+q=zeros(robot.ndof,length(q_J));
+q(6,:)=pi/2*q_J;
+q(4,:)=-pi/4*ones(size(q_J));
+qd=zeros(robot.ndof,length(q_J));qdd=qd;
+idntfcnTrjctry(6).t= vertcat(idntfcnTrjctry(6).t,time);
+idntfcnTrjctry(6).q= vertcat(idntfcnTrjctry(6).q,q);
+idntfcnTrjctry(6).qd= vertcat(idntfcnTrjctry(6).qd,qd);
+idntfcnTrjctry(6).qdd= vertcat(idntfcnTrjctry(6).qdd,qdd);
+
+% J5 traj
+time = 0:0.1:1;
+f=1;
+q_J = sin(2*pi*f*time);
+q=zeros(robot.ndof,length(q_J));
+q(5,:)=pi/2*q_J;
+q(4,:)=pi/4*ones(size(q_J));
+qd=zeros(robot.ndof,length(q_J));qdd=qd;
+idntfcnTrjctry(5).t = time;
+idntfcnTrjctry(5).q = q;
+idntfcnTrjctry(5).qd = qd;
+idntfcnTrjctry(5).qdd = qdd;
+
+time = 0:0.1:1;
+f=1;
+q_J = sin(2*pi*f*time);
+q=zeros(robot.ndof,length(q_J));
+q(5,:)=pi/2*q_J;
+q(4,:)=-pi/4*ones(size(q_J));
+qd=zeros(robot.ndof,length(q_J));qdd=qd;
+idntfcnTrjctry(5).t = vertcat(idntfcnTrjctry(5).t,time);
+idntfcnTrjctry(5).q = vertcat(idntfcnTrjctry(5).q,q);
+idntfcnTrjctry(5).qd = vertcat(idntfcnTrjctry(5).qd,qd);
+idntfcnTrjctry(5).qdd = vertcat(idntfcnTrjctry(5).qdd,qdd);
+
+% J4 traj
+time = 0:0.1:1;
+f=1;
+q_J = sin(2*pi*f*time);
+q=zeros(robot.ndof,length(q_J));
+q(4,:)=pi/4*q_J;
+qd=zeros(robot.ndof,length(q_J));qdd=qd;
+idntfcnTrjctry(4).t = time;
+idntfcnTrjctry(4).q = q;
+idntfcnTrjctry(4).qd = qd;
+idntfcnTrjctry(4).qdd = qdd;
+
+% J3 traj
+time = 0:0.1:1;
+f=1;
+q_J = sin(2*pi*f*time);
+q=zeros(robot.ndof,length(q_J));
+q(3,:)=pi/4*q_J;
+qd=zeros(robot.ndof,length(q_J));qdd=qd;
+idntfcnTrjctry(3).t = time;
+idntfcnTrjctry(3).q = q;
+idntfcnTrjctry(3).qd = qd;
+idntfcnTrjctry(3).qdd = qdd;
+
+% J2 traj
+time = 0:0.1:1;
+f=1;
+q_J = sin(2*pi*f*time);
+q=zeros(robot.ndof,length(q_J));
+q(2,:)=pi/4*q_J;
+qd=zeros(robot.ndof,length(q_J));qdd=qd;
+idntfcnTrjctry(2).t = time;
+idntfcnTrjctry(2).q = q;
+idntfcnTrjctry(2).qd = qd;
+idntfcnTrjctry(2).qdd = qdd;
+
+% J1 traj
+time = 0:0.1:1;
+f=1;
+q_J = sin(2*pi*f*time);
+q=zeros(robot.ndof,length(q_J));
+q(1,:)=pi/4*q_J;
+qd=zeros(robot.ndof,length(q_J));qdd=qd;
+idntfcnTrjctry(1).t = time;
+idntfcnTrjctry(1).q = q;
+idntfcnTrjctry(1).qd = qd;
+idntfcnTrjctry(1).qdd = qdd;
+
+% pi -> [m;mc;I] 10 element 
+[nLnkPrms, nLnks] = size(robot.pi);
+robot_pi = reshape(robot.pi, [nLnkPrms*nLnks, 1]);
+for i=1:robot.ndof
+    q = idntfcnTrjctry(i).q;
+    qd = idntfcnTrjctry(i).qd;
+    qdd = idntfcnTrjctry(i).qdd;
+    [nRow,nCol] = size(idntfcnTrjctry(i).qd);
+    for j = 1:nRow/robot.ndof
+        for k = 1:nCol
+            standard_regressor_func = sprintf('standard_regressor_%s',opt.robotName);
+            regressor = feval(standard_regressor_func,q(robot.ndof*(j-1)+1:robot.ndof*j,k),...
+                qd(robot.ndof*(j-1)+1:robot.ndof*j,k),qdd(robot.ndof*(j-1)+1:robot.ndof*j,k));
+            tau_Full=regressor*robot_pi;
+            tau(j,k) = tau_Full(i);
+        end
+    end
+    idntfcnTrjctry(i).tau = tau;
+end

get_Kinematics_R1000_DVT.m

@@ -1,4 +1,4 @@
-function robot = get_Kinematics(robot, opt)
+function robot = get_Kinematics_R1000_DVT(robot, opt)
 if(opt.Isreal)
     switch opt.KM_method
         case 'SCREW'

get_Kinematics_R1000_EVT.m

@@ -1,4 +1,4 @@
-function robot = get_Kinematics(robot, opt)
+function robot = get_Kinematics_R1000_EVT(robot, opt)
 if(opt.Isreal)
     switch opt.KM_method
         case 'SCREW'

get_baseParams.m

@@ -14,7 +14,7 @@ q2d_max = 6*pi*ones(ndof,1);
 % Find relation between independent columns and dependent columns
 % -----------------------------------------------------------------------
 % Get observation matrix of identifiable paramters
-W = []; isSixAxisFTSensor =opt.isSixAxisFTSensor; isJointTorqueSensor =opt.isJointTorqueSensor;  
+W = [];   
 for i = 1:25
     q_rnd = q_min + (q_max - q_min).*rand(ndof,1);
     qd_rnd = -qd_max + 2*qd_max.*rand(ndof,1);
@@ -22,12 +22,12 @@ for i = 1:25
     
     if includeMotorDynamics
 %         Y = regressorWithMotorDynamics(q_rnd,qd_rnd,q2d_rnd);
-    elseif isJointTorqueSensor
+    elseif opt.isJointTorqueSensor
         standard_regressor_func = sprintf('standard_regressor_%s',opt.robotName);
         Y = feval(standard_regressor_func, q_rnd,qd_rnd,q2d_rnd);
-        joint_idex = ndof-2;
+%         joint_idex = ndof-2;
-        Y = Y(joint_idex:end,:);
+%         Y = Y(joint_idex:end,:);
-    elseif isSixAxisFTSensor
+    elseif opt.isSixAxisFTSensor
         regressor_func = sprintf('regressor_%s',opt.robotName);
         Y = feval(regressor_func, q_rnd,qd_rnd,q2d_rnd);
         joint_idex = ndof-2;
@@ -99,6 +99,21 @@ baseQR.baseParams = pi_lgr_base;
 robot.baseQR = baseQR;
 robot.baseQR.regressor = robot.regressor.K*robot.baseQR.permutationMatrix(:,1:robot.baseQR.numberOfBaseParameters);
 
+% Get observation matrix of identifiable paramters
+for j = 1:robot.ndof
+    W = [];
+    for i = 1:25
+        q_rnd = q_min + (q_max - q_min).*rand(ndof,1);
+        qd_rnd = -qd_max + 2*qd_max.*rand(ndof,1);
+        q2d_rnd = -q2d_max + 2*q2d_max.*rand(ndof,1);
+        standard_regressor_func = sprintf('standard_regressor_%s',opt.robotName);
+        Y = feval(standard_regressor_func, q_rnd,qd_rnd,q2d_rnd);
+        W = vertcat(W,Y(j,:));
+    end
+    % matrix W has rank qr_rank which is number number of base parameters 
+    robot.baseQR.rank(j) = rank(W); 
+end
+
 if(opt.reGenerate)
     tic
     disp('compiling robot.baseQR.regressor');

get_baseParams_rank.m

@@ -0,0 +1,24 @@
+% ------------------------------------------------------------------------
+% Set limits on posistion and velocities
+% ------------------------------------------------------------------------
+q_min = -pi*ones(ndof,1);
+q_max = pi*ones(ndof,1);
+qd_max = 3*pi*ones(ndof,1);
+q2d_max = 6*pi*ones(ndof,1); 
+% -----------------------------------------------------------------------
+% Find relation between independent columns and dependent columns
+% -----------------------------------------------------------------------
+% Get observation matrix of identifiable paramters
+for j = 1:robot.ndof
+    W = [];
+    for i = 1:25
+        q_rnd = q_min + (q_max - q_min).*rand(ndof,1);
+        qd_rnd = -qd_max + 2*qd_max.*rand(ndof,1);
+        q2d_rnd = -q2d_max + 2*q2d_max.*rand(ndof,1);
+        standard_regressor_func = sprintf('standard_regressor_%s',opt.robotName);
+        Y = feval(standard_regressor_func, q_rnd,qd_rnd,q2d_rnd);
+        W = vertcat(W,Y(j,:));
+    end
+    % matrix W has rank qr_rank which is number number of base parameters 
+    robot.baseQR.rank(i) = rank(W); 
+end

get_robot_R1000_DVT.m

@@ -1,4 +1,4 @@
-function robot = get_robot_R1000(theta,dtheta,ddtheta,file,opt)
+function robot = get_robot_R1000_DVT(theta,dtheta,ddtheta,file,opt)
 switch opt.robot_def
     case 'direct'
         ndof = 9;