101 lines
3.6 KiB
Matlab
101 lines
3.6 KiB
Matlab
%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("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")
|
|
posDir = [1,1,1,1,1,1,1,-1,1];
|
|
isCurrentSensor = true;
|
|
% J9 traj
|
|
i = 9;
|
|
fileData=eval(strcat('fileData', num2str(i)));
|
|
data = fileData.data;
|
|
dataLength = length(data);
|
|
d1Length = floor(length(data) / 2);
|
|
% idntfcnTrjctry(i).tau = (data(1:d1Length,i+1+11*2)*gearRatio(i)*motorConstant(i)+...
|
|
% data(d1Length+1:end,i+1+11*2)*gearRatio(i)*motorConstant(i))/2;
|
|
idntfcnTrjctry(i).tau = data(:,i+1+11*2)*gearRatio(i)*motorConstant(i);
|
|
idntfcnTrjctry(i).tau = idntfcnTrjctry(i).tau';
|
|
|
|
q=zeros(robot.ndof,dataLength);qd=q;qdd=qd;
|
|
for j=1:robot.ndof
|
|
if(j==i||j==i-1)
|
|
q(j,:)=data(:,j+1+11);
|
|
continue;
|
|
end
|
|
q(j,:) = mean(data(:,j+1+11),1);
|
|
end
|
|
idntfcnTrjctry(i).q = q;
|
|
idntfcnTrjctry(i).qd = qd;
|
|
idntfcnTrjctry(i).qdd = qdd;
|
|
|
|
% J8 to J2 traj
|
|
for i=8:-1:2
|
|
fileData=eval(strcat('fileData', num2str(i)));
|
|
data = fileData.data;
|
|
dataLength = length(data);
|
|
d1Length = floor(length(data) / 2);
|
|
if(isCurrentSensor)
|
|
idntfcnTrjctry(i).tau = data(:,i+1+11*2)*gearRatio(i)*motorConstant(i);
|
|
else
|
|
idntfcnTrjctry(i).tau = data(:,6*(i+1)+11*3)*sensorDir(i);
|
|
end
|
|
idntfcnTrjctry(i).tau = idntfcnTrjctry(i).tau';
|
|
q=zeros(robot.ndof,dataLength);qd=q;qdd=qd;
|
|
for j=1:robot.ndof
|
|
if((j==i||j==i-1)&&i~=2) %hack i=2
|
|
q(j,:)=data(:,j+1+11);
|
|
continue;
|
|
end
|
|
if((i==2&&j==6)||(i==2&&j==2))
|
|
q(j,:)=data(:,j+1+11);
|
|
continue;
|
|
end
|
|
q(j,:) = mean(data(:,j+1+11),1);
|
|
end
|
|
idntfcnTrjctry(i).q = q;
|
|
idntfcnTrjctry(i).qd = qd;
|
|
idntfcnTrjctry(i).qdd = qdd;
|
|
end
|
|
|
|
% Reduce data size
|
|
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, 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);
|
|
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);
|
|
end
|
|
|
|
%hack
|
|
% 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);
|
|
% robot_pi = reshape(robot.pi, [nLnkPrms*nLnks, 1]);
|
|
% for i=2:robot.ndof
|
|
% q = idntfcnTrjctry(i).q;
|
|
% qd = idntfcnTrjctry(i).qd;
|
|
% qdd = idntfcnTrjctry(i).qdd;
|
|
% [nRow,nCol] = size(idntfcnTrjctry(i).qd);
|
|
% for j = 1:nRow/robot.ndof
|
|
% for k = 1:nCol
|
|
% standard_regressor_func = sprintf('standard_regressor_%s',opt.robotName);
|
|
% regressor = feval(standard_regressor_func,q(robot.ndof*(j-1)+1:robot.ndof*j,k),...
|
|
% qd(robot.ndof*(j-1)+1:robot.ndof*j,k),qdd(robot.ndof*(j-1)+1:robot.ndof*j,k));
|
|
% tau_Full=regressor*robot_pi;
|
|
% tau(j,k) = tau_Full(i);
|
|
% end
|
|
% end
|
|
% Trjctry(i).tau = tau;
|
|
% end |