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

feature/R1000-identification #2

Merged
cosmic_power merged 48 commits from feature/R1000-identification into main 2024-12-16 15:25:53 +00:00
Conversation 0 Commits 48 Files Changed 129 +52 -13
2 changed files with 52 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 49a40c0957 - Show all commits

22
get_GCTraj_R1000_DVT.m
View File

@ -1,8 +1,8 @@
%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");
sensorDir = [-1,1,-1,-1,-1,1,-1,1,1];
load("C:\GitLab\gtools\development_files\SLX_Development\R1000\SA System\lab12.mat");
posDir = [1,1,1,1,1,1,1,-1,1];
% J9 traj
i = 9;
@ -21,10 +21,6 @@ for j=1:robot.ndof
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;
@ -60,10 +56,10 @@ end
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);
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);
continue;
end
dataLength = length(idntfcnTrjctry(i).tau);
@ -74,9 +70,9 @@ for i=robot.ndof:-1:2
end
%hack
for i=robot.ndof:-1:2
idntfcnTrjctry(i).q(8,:) = - idntfcnTrjctry(i).q(8,:);
end
% 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);

43
plotData_test.m Normal file
View File

@ -0,0 +1,43 @@
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("C:\GitLab\gtools\development_files\SLX_Development\R1000\SA System\lab12.mat");
posDir = [1,1,1,1,1,1,1,-1,1];
% J9 traj
for i=7
fileData=eval(strcat('fileData', num2str(i)));
data = fileData.data;
dataLength = length(data);
figure(i)
d1Length = floor(length(data) / 2);
% Hack: change J8 dir
%data(:,8+1+11) = -data(:,8+1+11);
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;
hold off;
title(['J' num2str(i) ' Gravity Model']);xlabel('X Pos/rad');ylabel('Y Pos/rad')
end
%%
% should run identifcation program firstly
resolution = 20;
tau_estimate=[];
for k=7
qx = idntfcnTrjctry(k).q(k,:);
qy = idntfcnTrjctry(k).q(k-1,:);
[X, Y] = meshgrid(linspace(min(qx), max(qx), resolution), linspace(min(qy), max(qy), resolution));
q = mean(idntfcnTrjctry(k).q,2);qd=zeros(9,1);qdd=zeros(9,1);
for i = 1 : length(X)
for j = 1 : length(Y)
q(k) = X(i,j);q(k-1) = Y(i,j);
base_regressor_func = sprintf('base_regressor_%s',opt.robotName);
Yb = feval(base_regressor_func,q,qd,qdd,robot.baseQR);
torque=Yb*robot.sol.pib_MLS;
tau_estimate(i,j)=torque(k);
end
end
end
hold on;
mesh(X,Y,tau_estimate,tau_estimate, 'FaceAlpha', 0.0)