add sixaxis Ft

Identification_main.m

@@ -20,15 +20,15 @@ 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);
 
-Jaco=JacobianSpace(robot.slist(:,1:7),theta(1:7));
-R1000_Dynamics_num;
+% Jaco=JacobianSpace(robot.slist(:,1:7),theta(1:7));
+% R1000_Dynamics_num;
 % test_SixAxisFT;
 % R1000_Dynamics;
-% robot = get_velocity(robot, opt);
-% robot = get_regressor(robot,opt);
+robot = get_velocity(robot, opt);
+robot = get_regressor(robot,opt);
 % % symbol matched
 % % verify_regressor_R1000;
-% robot = get_baseParams(robot, opt);
-% % robot = estimate_dyn(robot,opt);
-% % robot = estimate_dyn_form_data(robot,opt);
-% robot = estimate_dyn_MLS(robot,opt);
+robot = get_baseParams(robot, opt);
+% robot = estimate_dyn(robot,opt);
+% robot = estimate_dyn_form_data(robot,opt);
+robot = estimate_dyn_MLS(robot,opt);

R1000_Dynamics_num.m

@@ -15,7 +15,7 @@ link_mass = robot.m;
 com_pos = robot.com;
 link_inertia = robot.I;
 
-thetalist = [zero_;zero_;zero_;zero_;zero_;pi/3*ones_;zero_;zero_;zero_]';
+thetalist = [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_]';
 
@@ -138,12 +138,14 @@ Mlist_ED = cat(3, Mlist_ED, M);
 %TODO: Get Slist form DH table method
 % RRRRRRRRP
 Slist=robot.slist;
-exf=[0;0;0;0;0;0];
+exf=[0;0;0;0;1;0];
 
 for i = 1:length(thetalist)
     [V1(:,:, i),Vd1(:,:, i),Adgab_mat(:,:,:,i),Fmat(:,:,i),taumat(:,i)] ...
         = InverseDynamics_debug(thetalist(i,:)', dthetalist(i,:)', ddthetalist(i,:)', ...
-        [0;0;-9.806], exf, Mlist_CG, Glist, Slist);
+        [0;0;0], exf, Mlist_CG, Glist, Slist);
+    %         [0;0;-9.806], exf, Mlist_CG, Glist, Slist);
+
     G(:,:,:,i) = FKinSpaceExpand(Mlist_CG, Slist, thetalist(i,:)');
     T(:,:,:,i)=FKinSpaceExpand(Mlist_ED, Slist, thetalist(i,:)');
     %Want to get the result from TC_delta, which means F at CG represent under frame at the last origin  

get_GCTraj_R1000_DVT.m

@@ -1,12 +1,12 @@
 %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];
+% motorConstant = [0.21*2.5,0.21*2.5,0.128,0.119,0.094,0.094,0.094,0.099,0.031];
 % DVT1-2
-% motorConstant = [0.21,0.21,0.128,0.119,0.094,0.094,0.094,0.099,0.031];
+motorConstant = [0.21,0.21,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("C:\Users\cosmicpower\AppData\Roaming\123pan\1833128421\123同步文件夹\R1000-GC-Data\large_range_lab1.mat");
 % load("D:\1833128421\123同步文件夹\R1000-GC-Data\large_range_lab1.mat")
-load("D:\1833128421\123同步文件夹\R1000-GC-Data\large_range_lab1.mat");
+load("D:\1833128421\123同步文件夹\R1000-GC-Data\DVT1-2\DVT1-2-GC-6axisok\DVT1-2-GC-6axisok.mat");
 posDir = [1,1,1,1,1,1,1,-1,1];
 isCurrentSensor = true;
 % J9 traj
@@ -78,6 +78,17 @@ for i=robot.ndof:-1:2
     idntfcnTrjctry(i).qdd = idntfcnTrjctry(i).qdd(:,mod(1:dataLength, 400) == 0);
 end
 
+% isoutlier
+for i=robot.ndof:-1:2
+    detect  = isoutlier(idntfcnTrjctry(i).tau);
+    for j=1:length(detect)
+        if(detect(j)==1)
+           %tricky
+           idntfcnTrjctry(i).tau(j) = idntfcnTrjctry(i).tau(j-10);
+        end
+    end
+end
+
 %hack
 % for i=robot.ndof:-1:2
 %     idntfcnTrjctry(i).q(8,:) = - idntfcnTrjctry(i).q(8,:); 

plotData_current.m

@@ -1,13 +1,30 @@
 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];
+% motorConstant = [0.21*2.5,0.21*2.5,0.128,0.119,0.094,0.094,0.094,0.099,0.031];
 % DVT1-2
-% motorConstant = [0.21,0.21,0.128,0.119,0.094,0.094,0.094,0.099,0.031];
+motorConstant = [0.21,0.21,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("C:\Users\cosmicpower\AppData\Roaming\123pan\1833128421\123同步文件夹\R1000-GC-Data\large_range_lab1.mat");
 posDir = [1,1,1,1,1,1,1,-1,1];
 
 % J9 traj
-for i=8
+% hack
+% Filter outliers using moving median filter
+windowSize = 21; % Must be odd
+data_filtered = zeros([length(data(:,(6*(2+1))+11*3)),robot.ndof]);
+for i= robot.ndof:-1:2
+data_filtered(:,i) = data(:,(6*(i+1))+11*3);
+% Apply moving median filter
+medianFiltered = movmedian(data_filtered(:,i), windowSize);
+% Find outliers using median absolute deviation
+medianVal = median(data_filtered(:,i));
+madVal = median(abs(data_filtered(:,i) - medianVal));
+threshold = 3; % Number of MAD deviations to consider as outlier
+outlierIdx = abs(data_filtered(:,i) - medianVal) > threshold * madVal;
+% Replace outliers with median filtered values
+data_filtered(outlierIdx,i) = medianFiltered(outlierIdx);
+end
+
+for i=6
     fileData=eval(strcat('fileData', num2str(i)));
     data = fileData.data;
     dataLength = length(data);
@@ -18,11 +35,11 @@ for i=8
     if i==2
         plot3(data(1:d1Length,i+1+11),data(1:d1Length,6+1+11),data(1:d1Length,i+1+11*2),'r');    hold on;
         plot3(data(d1Length:end,i+1+11),data(d1Length:end,6+1+11),data(d1Length:end,i+1+11*2),'b');    hold on;
-%         plot3(data(:,i+1+11),data(:,6+1+11),data(:,(6*(i+1))+11*3)*sensorDir(i)/(gearRatio(i)*motorConstant(i)),'m');    hold on;
+        plot3(data(:,i+1+11),data(:,6+1+11),data(:,(6*(i+1))+11*3)*sensorDir(i)/(gearRatio(i)*motorConstant(i)),'m');    hold on;
     else
         plot3(data(1:d1Length,i+1+11),data(1:d1Length,i+11),data(1:d1Length,i+1+11*2),'r');    hold on;
         plot3(data(d1Length:end,i+1+11),data(d1Length:end,i+11),data(d1Length:end,i+1+11*2),'b');    hold on;
-%         plot3(data(:,i+1+11),data(:,i+11),data(:,(6*(i+1))+11*3)*sensorDir(i)/(gearRatio(i)*motorConstant(i)),'m');    hold on;
+        plot3(data(:,i+1+11),data(:,i+11),data_filtered*sensorDir(i)/(gearRatio(i)*motorConstant(i)),'m');    hold on;
     end
     hold off;
     title(['J' num2str(i) ' Gravity Model']);
@@ -32,7 +49,7 @@ end
 % should run identifcation program firstly
 resolution = 20;
 tau_estimate=[];
-for k=8
+for k=6
     if k ==2
         qx = idntfcnTrjctry(k).q(k,:);
         qy = idntfcnTrjctry(k).q(6,:);

plotData_test.m

@@ -1,11 +1,31 @@
 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];
+% DVT1-1
+% sensorDir = [-1,1,-1,-1,-1,1,-1,1,1];
+% DVT1-2
+sensorDir = [-1,1,-1,-1,1,-1,-1,1,1];
 % load("D:\1833128421\123同步文件夹\R1000-GC-Data\lab12.mat");
 posDir = [1,1,1,1,1,1,1,-1,1];
 
 % J9 traj
-for i=6
+% hack
+% Filter outliers using moving median filter
+windowSize = 21; % Must be odd
+data_filtered = zeros([length(data(:,(6*(2+1))+11*3)),robot.ndof]);
+for i= robot.ndof:-1:2
+data_filtered(:,i) = data(:,(6*(i+1))+11*3);
+% Apply moving median filter
+medianFiltered = movmedian(data_filtered(:,i), windowSize);
+% Find outliers using median absolute deviation
+medianVal = median(data_filtered(:,i));
+madVal = median(abs(data_filtered(:,i) - medianVal));
+threshold = 3; % Number of MAD deviations to consider as outlier
+outlierIdx = abs(data_filtered(:,i) - medianVal) > threshold * madVal;
+% Replace outliers with median filtered values
+data_filtered(outlierIdx,i) = medianFiltered(outlierIdx);
+end
+
+for i=5
     fileData=eval(strcat('fileData', num2str(i)));
     data = fileData.data;
     dataLength = length(data);
@@ -16,11 +36,12 @@ for i=6
     if i==2
         plot3(data(1:d1Length,i+1+11),data(1:d1Length,6+1+11),data(1:d1Length,i+1+11*2)*gearRatio(i)*motorConstant(i),'r');    hold on;
         plot3(data(d1Length:end,i+1+11),data(d1Length:end,6+1+11),data(d1Length:end,i+1+11*2)*gearRatio(i)*motorConstant(i),'b');    hold on;
-%         plot3(data(:,i+1+11),data(:,6+1+11),data(:,(6*(i+1))+11*3)*sensorDir(i),'m');    hold on;
+        plot3(data(:,i+1+11),data(:,6+1+11),data(:,(6*(i+1))+11*3)*sensorDir(i),'m');    hold on;
     else
         plot3(data(1:d1Length,i+1+11),data(1:d1Length,i+11),data(1:d1Length,i+1+11*2)*gearRatio(i)*motorConstant(i),'r');    hold on;
         plot3(data(d1Length:end,i+1+11),data(d1Length:end,i+11),data(d1Length:end,i+1+11*2)*gearRatio(i)*motorConstant(i),'b');    hold on;
 %         plot3(data(:,i+1+11),data(:,i+11),data(:,(6*(i+1))+11*3)*sensorDir(i),'m');    hold on;
+        plot3(data(:,i+1+11),data(:,i+11),data_filtered(:,i)*sensorDir(i),'m');    hold on;
     end
     hold off;
     title(['J' num2str(i) ' Gravity Model']);
@@ -30,7 +51,7 @@ end
 % should run identifcation program firstly
 resolution = 20;
 tau_estimate=[];
-for k=6
+for k=5
     if k ==2
         qx = idntfcnTrjctry(k).q(k,:);
         qy = idntfcnTrjctry(k).q(6,:);

test_SixAxisFT.m

@@ -0,0 +1,114 @@
+% test Six axis force torque sensor
+%% R1000
+N=9;
+% Dynamics parameters
+link_mass = robot.m;
+com_pos = robot.com;
+link_inertia = robot.I;
+
+%real traj
+load("D:\1833128421\123同步文件夹\SixAxisFT\SixAxisForceTest2.mat");
+index1 = 94470:115700;index2 = 228400:236700;index3 = 401500:414700;
+FT_delta = double(data{1}.Values.data([index1,index2,index3],:));
+Pos = double(data{2}.Values.data([index1,index2,index3],2:10));
+% resample
+dataLength = length(FT_delta);
+FT_delta = FT_delta(mod(1:dataLength, 400) == 0,:);
+Pos = Pos(mod(1:dataLength, 400) == 0,:);
+thetalist = Pos;dthetalist = zeros(size(Pos));ddthetalist=dthetalist;
+
+% Get general mass matrix
+Glist=[];
+for i = 1:N
+    link_inertia(:,:,i) = robot.Home.R(:,:,i)'*link_inertia(:,:,i)*robot.Home.R(:,:,i);
+    Gb= [link_inertia(:,:,i),zeros(3,3);zeros(3,3),link_mass(i)*diag([1,1,1])];
+    Glist = cat(3, Glist, Gb);
+end
+
+% get the CG at the world base frame
+com_pos_R1 = robot.com_pos_R1;
+com_pos_R2 = robot.com_pos_R2;
+ct=[];
+Mlist_CG_Base=[];
+for i = 1:N
+    if i == 1
+        ct(:,i) = com_pos_R1(:,i);
+    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.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));
+    Mlist_CG_Base = cat(3, Mlist_CG_Base, M_CG_Base);
+end
+% get the CG at the last GC frame
+Mlist_CG=[];
+for i = 1:N
+    if i == 1
+        Mlist_CG(:,:,i) = Mlist_CG_Base(:,:,i);
+    else
+        Mlist_CG(:,:,i) = TransInv(Mlist_CG_Base(:,:,i-1))*Mlist_CG_Base(:,:,i);
+    end
+end
+M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
+Mlist_CG = cat(3, Mlist_CG, M);
+
+
+% Get the end efforce transformation in each joint reference frame
+Mlist_ED = [];
+for i = 1:N
+    M = robot.T(:,:,i);
+    Mlist_ED = cat(3, Mlist_ED, M);
+end
+M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
+Mlist_ED = cat(3, Mlist_ED, M);
+
+%TODO: Get Slist form DH table method
+% RRRRRRRRP
+Slist=robot.slist;
+
+Vlinear=sym(zeros(3,3));
+J=sym(zeros(6,N));
+exf=[0;0;0;0;0;0];
+
+% real traj
+for i = 1:length(thetalist)
+    [V1(:,:, i),Vd1(:,:, i),Adgab_mat(:,:,:,i),Fmat(:,:,i),taumat(:,i)] ...
+        = InverseDynamics_debug(thetalist(i,:)', dthetalist(i,:)', ddthetalist(i,:)', ...
+        [0;0;-9.806], exf, Mlist_CG, Glist, Slist);
+    G(:,:,:,i) = FKinSpaceExpand(Mlist_CG, Slist, thetalist(i,:)');
+    T(:,:,:,i)=FKinSpaceExpand(Mlist_ED, Slist, thetalist(i,:)');
+    %Want to get the result from TC_delta, which means F at CG represent under frame at the last origin  
+    %why we need Mlist_ED
+    %please explain this more
+    F_Simpack(:,:,i) = getSimpackF(G(:,:,:,i),T(:,:,:,i),Mlist_ED,Fmat(:,:,i));
+end
+
+% init force and torque at home
+zero_ = zeros(size(Pos));
+thetalist = zero_;dthetalist = thetalist;ddthetalist=dthetalist;
+for i = 1:length(thetalist)
+    [V1(:,:, i),Vd1(:,:, i),Adgab_mat(:,:,:,i),Fmat(:,:,i),taumat(:,i)] ...
+        = InverseDynamics_debug(thetalist(i,:)', dthetalist(i,:)', ddthetalist(i,:)', ...
+        [0;0;-9.806], exf, Mlist_CG, Glist, Slist);
+    G(:,:,:,i) = FKinSpaceExpand(Mlist_CG, Slist, thetalist(i,:)');
+    T(:,:,:,i)=FKinSpaceExpand(Mlist_ED, Slist, thetalist(i,:)');
+    %Want to get the result from TC_delta, which means F at CG represent under frame at the last origin  
+    %why we need Mlist_ED
+    %please explain this more
+    F_Simpack_Init(:,:,i) = getSimpackF(G(:,:,:,i),T(:,:,:,i),Mlist_ED,Fmat(:,:,i));
+end
+
+for i = 1:length(thetalist)
+    F_Simpack_delta(:,:,i) = F_Simpack(:,:,i)-F_Simpack_Init(:,:,i);
+end
+% plot Torque
+% above 2020b 
+% F_Simpack = pagetranspose(F_Simpack);
+% below 2020b
+F_Simpack_delta = permute(F_Simpack_delta,[2 1 3]);
+plot(thetalist(:,6),-reshape(F_Simpack_delta(7,5,:),[1,length(F_Simpack_delta)]));
+% test = fileData6.data(:,(6*(8+1))+11*3-5);
+% test_time = fileData6.data(:,6+1+11);
+% hold on;plot(test_time,test)