add real data but testing

Identification_main.m

@@ -20,16 +20,13 @@ robot = feval(get_robot_func,theta,dtheta,ddtheta,file,opt);
 get_Kinematics_func = sprintf('get_Kinematics_%s',opt.robotName);
 robot = feval(get_Kinematics_func,robot,opt);
 
-R1000_Dynamics_num;
+% R1000_Dynamics_num;
 % R1000_Dynamics;
-% robot = get_velocity(robot, opt);
+robot = get_velocity(robot, opt);
-% robot = get_regressor(robot,opt);
+robot = get_regressor(robot,opt);
 % symbol matched
 % verify_regressor_R1000;
-% robot = get_baseParams(robot, opt);
+robot = get_baseParams(robot, opt);
-% readDataFile;
-% robot.posData = posData;
-% robot.currentData = currentData;
 % robot = estimate_dyn(robot,opt);
 % robot = estimate_dyn_form_data(robot,opt);
-% robot = estimate_dyn_MLS(robot,opt);
+robot = estimate_dyn_MLS(robot,opt);

R1000_Dynamics_num.m

@@ -1,21 +1,29 @@
 %% R1000
 N=9;
 % traj
-time = 0:0.01:1;
+time = 0:0.01:1-0.01;
 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));
-q_J = 0.3*ones(1,length(q_J));
+q_J = -0.3*ones(1,length(q_J));
+ones_ = ones(1,length(q_J));
+
 % Dynamics parameters
 link_mass = robot.m;
 com_pos = robot.com;
 link_inertia = robot.I;
 
-thetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;q_J]';
+% thetalist = [zero_;0.4807*ones_;-0.8717*ones_;0.3466*ones_;zero_;0.6965*ones_;-0.5224*ones_;0.3473*ones_;0.05*ones_]';
-dthetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
+% dthetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
-ddthetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
+% ddthetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
+
+%real traj
+get_GCTraj_R1000_DVT;
+thetalist = idntfcnTrjctry(6).q';
+dthetalist = idntfcnTrjctry(6).qd';
+ddthetalist = idntfcnTrjctry(6).qdd';
 
 % Get general mass matrix
 Glist=[];
@@ -99,7 +107,7 @@ for i = 1:N
     elseif i< 9
         ct(:,i) = ct(:,i-1)-com_pos_R2(:,i-1)+com_pos_R1(:,i);
     else
-        ct(:,i) = ct(:,i-1)-com_pos_R1(:,i-1)-[0;0;0.05896]+com_pos_R1(:,i);
+        ct(:,i) = ct(:,i-1)-com_pos_R1(:,i-1)-[0.3157;0;0.05896]+com_pos_R1(:,i);
     end
     robot.Home.com(:,i) = ct(:,i);
     M_CG_Base = RpToTrans(robot.Home.R(:,:,i),robot.Home.com(:,i));

estimate_dyn_MLS.m

@@ -2,14 +2,16 @@ function robot = estimate_dyn_MLS(robot,opt)
 % -------------------------------------------------------------------
 % Get datas
 % ------------------------------------------------------------------------
-get_GCTraj_R1000_EVT;
+get_GCTraj_func = sprintf('get_GCTraj_%s',opt.robotName);
+feval(get_GCTraj_func);
 % -------------------------------------------------------------------
 % Generate Regressors based on data
 % ------------------------------------------------------------------------
 drvGains = [];
 baseQR = robot.baseQR;
 
-for i = 1:1:robot.ndof
+for i = 2:1:robot.ndof
+% for i = robot.ndof
     q = idntfcnTrjctry(i).q;
     qd = idntfcnTrjctry(i).qd;
     qdd = idntfcnTrjctry(i).qdd;
@@ -37,7 +39,8 @@ end
 % Estimate parameters
 % ---------------------------------------------------------------------
 sol = struct;
-for i = robot.ndof:-1:1
+for i = robot.ndof:-1:2
+% for i = robot.ndof
     Wb = observationMatrix(i).Wb;
     Tau = observationMatrix(i).Tau;
     if i == robot.ndof

get_GCTraj_R1000_DVT.m

@@ -0,0 +1,99 @@
+%temp get DVT traj
+gearRatio = [100,100,120,100,100,80,50,100,1/(0.012/(2*pi))];
+motorConstant = [0.21*2.5,0.21*2.5,0.128,0.119,0.094,0.094,0.094,0.099,0.031];
+sensorDir = [-1,1,-1,-1,-1,1,1,1,1];
+load("G:\123pan\Downloads\重力补偿参数解析\matlab.mat");
+posDir = [1,1,1,1,1,1,1,-1,1];
+% J9 traj
+i = 9;
+fileData=eval(strcat('fileData', num2str(i)));
+data = fileData.data;
+dataLength = length(data);
+d1Length = floor(length(data) / 2);
+% idntfcnTrjctry(i).tau = (data(1:d1Length,i+1+11*2)*gearRatio(i)*motorConstant(i)+...
+%     data(d1Length+1:end,i+1+11*2)*gearRatio(i)*motorConstant(i))/2;
+idntfcnTrjctry(i).tau = data(:,i+1+11*2)*gearRatio(i)*motorConstant(i);
+idntfcnTrjctry(i).tau = idntfcnTrjctry(i).tau';
+
+q=zeros(robot.ndof,dataLength);qd=q;qdd=qd;
+for j=1:robot.ndof
+    if(j==i||j==i-1)
+        q(j,:)=data(:,j+1+11);
+        continue;
+    end
+%     if(j==8) %hack
+%         q(j,:)=-data(:,j+1+11);
+%         continue;
+%     end
+    q(j,:) = mean(data(:,j+1+11),1);
+end
+idntfcnTrjctry(i).q = q;
+idntfcnTrjctry(i).qd = qd;
+idntfcnTrjctry(i).qdd = qdd;
+
+% J8 to J2 traj
+for i=8:-1:2
+    fileData=eval(strcat('fileData', num2str(i)));
+    data = fileData.data;
+    dataLength = length(data);
+    d1Length = floor(length(data) / 2);
+    idntfcnTrjctry(i).tau = data(:,6*(i+1)+11*3)*sensorDir(i);
+    idntfcnTrjctry(i).tau = idntfcnTrjctry(i).tau';
+    q=zeros(robot.ndof,dataLength);qd=q;qdd=qd;
+    for j=1:robot.ndof
+        if((j==i||j==i-1)&&i~=2) %hack i=2
+            q(j,:)=data(:,j+1+11);
+            continue;
+        end
+        if((i==2&&j==6)||(i==2&&j==2))
+            q(j,:)=data(:,j+1+11);
+            continue;
+        end
+        q(j,:) = mean(data(:,j+1+11),1);
+    end
+    idntfcnTrjctry(i).q = q;
+    idntfcnTrjctry(i).qd = qd;
+    idntfcnTrjctry(i).qdd = qdd;
+end
+
+% Reduce data size
+for i=robot.ndof:-1:2
+    if i == 9 || i == 8 || i==2
+        dataLength = length(idntfcnTrjctry(i).tau);
+        idntfcnTrjctry(i).tau = idntfcnTrjctry(i).tau(mod(1:dataLength, 4000) == 0);
+        idntfcnTrjctry(i).q = idntfcnTrjctry(i).q(:,mod(1:dataLength, 4000) == 0);
+        idntfcnTrjctry(i).qd = idntfcnTrjctry(i).qd(:,mod(1:dataLength, 4000) == 0);
+        idntfcnTrjctry(i).qdd = idntfcnTrjctry(i).qdd(:,mod(1:dataLength, 4000) == 0);
+        continue;
+    end
+    dataLength = length(idntfcnTrjctry(i).tau);
+    idntfcnTrjctry(i).tau = idntfcnTrjctry(i).tau(mod(1:dataLength, 400) == 0);
+    idntfcnTrjctry(i).q = idntfcnTrjctry(i).q(:,mod(1:dataLength, 400) == 0);
+    idntfcnTrjctry(i).qd = idntfcnTrjctry(i).qd(:,mod(1:dataLength, 400) == 0);
+    idntfcnTrjctry(i).qdd = idntfcnTrjctry(i).qdd(:,mod(1:dataLength, 400) == 0);
+end
+
+%hack
+for i=robot.ndof:-1:2
+    idntfcnTrjctry(i).q(8,:) = - idntfcnTrjctry(i).q(8,:); 
+end
+
+% pi -> [m;mc;I] 10 element 
+% [nLnkPrms, nLnks] = size(robot.pi);
+% robot_pi = reshape(robot.pi, [nLnkPrms*nLnks, 1]);
+% for i=2: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
+%     Trjctry(i).tau = tau;
+% end

get_Kinematics_R1000_DVT.m

@@ -41,7 +41,7 @@ if(opt.Isreal)
                     ct(:,i) = ct(:,i-1)-com_pos_R2(:,i-1)+com_pos_R1(:,i);
                 else
                     % HACK
-                    ct(:,i) = ct(:,i-1)-com_pos_R1(:,i-1)-[0;0;0.05896]+com_pos_R1(:,i);
+                    ct(:,i) = ct(:,i-1)-com_pos_R1(:,i-1)-[0.3157;0;0.05896]+com_pos_R1(:,i);
                 end
                 robot.Home.com(:,i) = ct(:,i);
                 M_CG_Base = RpToTrans(robot.Home.R(:,:,i),robot.Home.com(:,i));
@@ -160,7 +160,7 @@ else
                     ct(:,i) = ct(:,i-1)-com_pos_R2(:,i-1)+com_pos_R1(:,i);
                 else
                     % HACK
-                    ct(:,i) = ct(:,i-1)-com_pos_R1(:,i-1)-[0;0;0.05896]+com_pos_R1(:,i);
+                    ct(:,i) = ct(:,i-1)-com_pos_R1(:,i-1)-[0.3157;0;0.05896]+com_pos_R1(:,i);
                 end
                 robot.Home.com(:,i) = ct(:,i);
                 M_CG_Base = RpToTrans(robot.Home.R(:,:,i),robot.Home.com(:,i));

get_robot_R1000_DVT.m

@@ -49,7 +49,7 @@ switch opt.robot_def
                 co(:,i) = co(:,i-1)+com_pos_R1(:,i)-com_pos_R2(:,i);
             else
                 %From base to ISA Origin
-                co(:,i) = co(:,i-1)-[0;0;0.05896];
+                co(:,i) = co(:,i-1)-[0.3157;0;0.05896];
             end
         end
         co = [zeros(3,1),co];

untitled2.m

@@ -49,7 +49,7 @@ for i = 1:9
     elseif i< 9
         ct(:,i) = ct(:,i-1)-com_pos_R2(:,i-1)+com_pos_R1(:,i);
     else
-        ct(:,i) = ct(:,i-1)-com_pos_R1(:,i-1)-[0;0;0.05896]+com_pos_R1(:,i);
+        ct(:,i) = ct(:,i-1)-com_pos_R1(:,i-1)-[0.3;0;0.05896]+com_pos_R1(:,i);
     end
 end
 plot3(ct(1,:),ct(2,:),ct(3,:),'o','Color','r')
@@ -65,7 +65,7 @@ for i = 1:8
         co(:,i) = co(:,i-1)+com_pos_R1(:,i)-com_pos_R2(:,i);
     else
         %From base to ISA Origin
-        co(:,i) = co(:,i-1)-[0;0;0.05896];
+        co(:,i) = co(:,i-1)-[0.3;0;0.05896];
     end
 end
 co = [zeros(3,1),co];