Robotics
/
IRDYn
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

filter DVT1-2

This commit is contained in:
cosmic_power 2025-01-06 19:21:02 +08:00
parent 960e66ba35
commit 72cb928dae
9 changed files with 75 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
Identification_main.m
View File

@ -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);

8
R1000_Dynamics_num.m
View File

@ -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

BIN
figure2/J3 Gravity Model.jpg
View File

Binary file not shown.
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 89 KiB

After

Width:  |  Height:  |  Size: 61 KiB

BIN
figure2/J4 Gravity Model.jpg
View File

Binary file not shown.
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 103 KiB

After

Width:  |  Height:  |  Size: 59 KiB

BIN
figure2/J5 Gravity Model.jpg
View File

Binary file not shown.
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 73 KiB

After

Width:  |  Height:  |  Size: 53 KiB

BIN
figure2/J6 Gravity Model.jpg
View File

Binary file not shown.
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 91 KiB

After

Width:  |  Height:  |  Size: 55 KiB

17
get_GCTraj_R1000_DVT.m
View File

@ -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,:);

29
plotData_current.m
View File

@ -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,:);

29
plotData_test.m
View File

@ -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,:);