E1gen.m

@@ -1,5 +0,0 @@
-function E=E1gen(A,i,j)
-n=size(A); %求矩阵A的行数和列数
-m=min(n); %获取矩阵行数和列数中的最小值
-E=eye(m); %产生单位对角阵
-E(i,i)=0; E(j,j)=0; E(i,j)=1; E(j,i)=1;

estimate_dyn_MLS.asv

@@ -1,110 +0,0 @@
-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(:,15-observationMatrix(i).rank+1"))*Tau;
-        pib_MLS = [];
-    elseif i > 1
-        pib_OLS=pinv(Wb(:,15-observationMatrix(i).rank+1:15-observationMatrix(i+1).rank))*...
-            (-Wb(:,15-observationMatrix(i+1).rank+1)*pib_MLS+Tau);
-    else
-        break;
-    end
-    pifrctn_OLS = 0;
-    pib_MLS = [pib_OLS;pib_MLS];
-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

estimate_dyn_MLS.m

@@ -37,23 +37,24 @@ end
 % Estimate parameters
 % ---------------------------------------------------------------------
 sol = struct;
-for i = 9:-1:1
+for i = robot.ndof:-1:1
     Wb = observationMatrix(i).Wb;
     Tau = observationMatrix(i).Tau;
-%     [nRow,nCol] = size(Wb);
+    if i == robot.ndof
-    if i == 9
+        pib_OLS=pinv(Wb(:,baseQR.numberOfBaseParameters-observationMatrix(i).rank+1:end))*Tau;
-        pib_OLS=pinv(Wb(:,15-observationMatrix(i).rank+1:end))*Tau;
         pib_MLS = [];
     elseif i > 1
-        pib_OLS=pinv(Wb(:,15-observationMatrix(i).rank+1:15-observationMatrix(i+1).rank))*...
+        pib_OLS=pinv(Wb(:,baseQR.numberOfBaseParameters-observationMatrix(i).rank+1:...
-            (-Wb(:,15-observationMatrix(i+1).rank+1:end)*pib_MLS+Tau);
+            baseQR.numberOfBaseParameters-observationMatrix(i+1).rank))*...
+            (-Wb(:,baseQR.numberOfBaseParameters-observationMatrix(i+1).rank+1:end)*pib_MLS+Tau);
     else
         break;
     end
     pifrctn_OLS = 0;
     pib_MLS = [pib_OLS;pib_MLS];
 end
-a=1
+sol.pib_MLS = pib_MLS;
+robot.sol = sol;
 % method = 'OLS';
 % if strcmp(method, 'OLS')
 %     % Usual least squares

getGravityForce.asv

@@ -1,30 +0,0 @@
-time = 0:0.01:1;
-f=1;    
-q_J = sin(2*pi*f*time);
-qd_J = (2*pi*f)*cos(2*pi*f*time);
-qdd_J = -(2*pi*f)^2*sin(2*pi*f*time);
-zero_ = zeros(1,length(q_J));
-
-
-% thetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
-thetalist = zeros(N,1);
-
-Mlist_CG = robot.kine.Mlist_CG;
-Slist=robot.slist;
-
-% Get general mass matrix
-Glist=[];
-for i = 1:N
-    Gb= [link_inertia(:,:,i),zeros(3,3);zeros(3,3),link_mass(i)*diag([1,1,1])];
-    Glist = cat(3, Glist, Gb);
-end
-gravity = [0;0;-9.806];
-
-for i = 1:N
-gravityForces(:,i) = Glist(:,:,i)*[zeros(3,1);gravity];
-Jacoblist(:,:,i) = JacobianSpace(Slist(:,1:i),thetalist(1:i));
-end
-
-for i = N:-1:1
-    gravityTorques(i) = transpose(Jacoblist(:,:,i))*gravityForces(:,i);
-end