58 lines
2.3 KiB
Matlab
58 lines
2.3 KiB
Matlab
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("D:\1833128421\123同步文件夹\R1000-GC-Data\lab12.mat");
|
|
posDir = [1,1,1,1,1,1,1,-1,1];
|
|
|
|
% J9 traj
|
|
for i=9
|
|
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);
|
|
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;
|
|
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;
|
|
end
|
|
hold off;
|
|
title(['J' num2str(i) ' Gravity Model']);
|
|
xlabel('X Pos/rad');ylabel('Y Pos/rad');zlabel('Current /A')
|
|
end
|
|
%%
|
|
% should run identifcation program firstly
|
|
resolution = 20;
|
|
tau_estimate=[];
|
|
for k=9
|
|
if k ==2
|
|
qx = idntfcnTrjctry(k).q(k,:);
|
|
qy = idntfcnTrjctry(k).q(6,:);
|
|
else
|
|
qx = idntfcnTrjctry(k).q(k,:);
|
|
qy = idntfcnTrjctry(k).q(k-1,:);
|
|
end
|
|
[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;
|
|
current_estimate(i,j)=torque(k)/(gearRatio(k)*motorConstant(k));
|
|
end
|
|
end
|
|
end
|
|
hold on;
|
|
mesh(X,Y,current_estimate,current_estimate, 'FaceAlpha', 0.0)
|
|
%%
|
|
% saveas(gcf, ['./figure/' 'J' num2str(k) ' Gravity Model' '.jpg'])
|
|
|