From 49a40c0957a6afc813362f4db01d30151ab5ec15 Mon Sep 17 00:00:00 2001 From: cosmic_power Date: Mon, 16 Dec 2024 22:09:04 +0800 Subject: [PATCH] add plotData --- get_GCTraj_R1000_DVT.m | 22 +++++++++------------ plotData_test.m | 43 ++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 52 insertions(+), 13 deletions(-) create mode 100644 plotData_test.m diff --git a/get_GCTraj_R1000_DVT.m b/get_GCTraj_R1000_DVT.m index ced97a8..64cceee 100644 --- a/get_GCTraj_R1000_DVT.m +++ b/get_GCTraj_R1000_DVT.m @@ -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); diff --git a/plotData_test.m b/plotData_test.m new file mode 100644 index 0000000..7a0ed08 --- /dev/null +++ b/plotData_test.m @@ -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) +