add sixaxis Ft
This commit is contained in:
cosmic_power
parent
72cb928dae
commit
4da2d08ae4
|
|
@ -0,0 +1,114 @@
|
||||||
|
% test Six axis force torque sensor
|
||||||
|
%% R1000
|
||||||
|
N=9;
|
||||||
|
% Dynamics parameters
|
||||||
|
link_mass = robot.m;
|
||||||
|
com_pos = robot.com;
|
||||||
|
link_inertia = robot.I;
|
||||||
|
|
||||||
|
%real traj
|
||||||
|
load("D:\1833128421\123同步文件夹\SixAxisFT\SixAxisForceTest2.mat");
|
||||||
|
index1 = 94470:115700;index2 = 228400:236700;index3 = 401500:414700;
|
||||||
|
FT_delta = double(data{1}.Values.data([index1,index2,index3],:));
|
||||||
|
Pos = double(data{2}.Values.data([index1,index2,index3],2:10));
|
||||||
|
% resample
|
||||||
|
dataLength = length(FT_delta);
|
||||||
|
FT_delta = FT_delta(mod(1:dataLength, 400) == 0,:);
|
||||||
|
Pos = Pos(mod(1:dataLength, 400) == 0,:);
|
||||||
|
thetalist = Pos;dthetalist = zeros(size(Pos));ddthetalist=dthetalist;
|
||||||
|
|
||||||
|
% Get general mass matrix
|
||||||
|
Glist=[];
|
||||||
|
for i = 1:N
|
||||||
|
link_inertia(:,:,i) = robot.Home.R(:,:,i)'*link_inertia(:,:,i)*robot.Home.R(:,:,i);
|
||||||
|
Gb= [link_inertia(:,:,i),zeros(3,3);zeros(3,3),link_mass(i)*diag([1,1,1])];
|
||||||
|
Glist = cat(3, Glist, Gb);
|
||||||
|
end
|
||||||
|
|
||||||
|
% get the CG at the world base frame
|
||||||
|
com_pos_R1 = robot.com_pos_R1;
|
||||||
|
com_pos_R2 = robot.com_pos_R2;
|
||||||
|
ct=[];
|
||||||
|
Mlist_CG_Base=[];
|
||||||
|
for i = 1:N
|
||||||
|
if i == 1
|
||||||
|
ct(:,i) = com_pos_R1(:,i);
|
||||||
|
elseif i< 9
|
||||||
|
ct(:,i) = ct(:,i-1)-com_pos_R2(:,i-1)+com_pos_R1(:,i);
|
||||||
|
else
|
||||||
|
ct(:,i) = ct(:,i-1)-com_pos_R1(:,i-1)-[0.3157;0;0.05896]+com_pos_R1(:,i);
|
||||||
|
end
|
||||||
|
robot.Home.com(:,i) = ct(:,i);
|
||||||
|
M_CG_Base = RpToTrans(robot.Home.R(:,:,i),robot.Home.com(:,i));
|
||||||
|
Mlist_CG_Base = cat(3, Mlist_CG_Base, M_CG_Base);
|
||||||
|
end
|
||||||
|
% get the CG at the last GC frame
|
||||||
|
Mlist_CG=[];
|
||||||
|
for i = 1:N
|
||||||
|
if i == 1
|
||||||
|
Mlist_CG(:,:,i) = Mlist_CG_Base(:,:,i);
|
||||||
|
else
|
||||||
|
Mlist_CG(:,:,i) = TransInv(Mlist_CG_Base(:,:,i-1))*Mlist_CG_Base(:,:,i);
|
||||||
|
end
|
||||||
|
end
|
||||||
|
M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
|
||||||
|
Mlist_CG = cat(3, Mlist_CG, M);
|
||||||
|
|
||||||
|
|
||||||
|
% Get the end efforce transformation in each joint reference frame
|
||||||
|
Mlist_ED = [];
|
||||||
|
for i = 1:N
|
||||||
|
M = robot.T(:,:,i);
|
||||||
|
Mlist_ED = cat(3, Mlist_ED, M);
|
||||||
|
end
|
||||||
|
M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
|
||||||
|
Mlist_ED = cat(3, Mlist_ED, M);
|
||||||
|
|
||||||
|
%TODO: Get Slist form DH table method
|
||||||
|
% RRRRRRRRP
|
||||||
|
Slist=robot.slist;
|
||||||
|
|
||||||
|
Vlinear=sym(zeros(3,3));
|
||||||
|
J=sym(zeros(6,N));
|
||||||
|
exf=[0;0;0;0;0;0];
|
||||||
|
|
||||||
|
% real traj
|
||||||
|
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);
|
||||||
|
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
|
||||||
|
%why we need Mlist_ED
|
||||||
|
%please explain this more
|
||||||
|
F_Simpack(:,:,i) = getSimpackF(G(:,:,:,i),T(:,:,:,i),Mlist_ED,Fmat(:,:,i));
|
||||||
|
end
|
||||||
|
|
||||||
|
% init force and torque at home
|
||||||
|
zero_ = zeros(size(Pos));
|
||||||
|
thetalist = zero_;dthetalist = thetalist;ddthetalist=dthetalist;
|
||||||
|
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);
|
||||||
|
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
|
||||||
|
%why we need Mlist_ED
|
||||||
|
%please explain this more
|
||||||
|
F_Simpack_Init(:,:,i) = getSimpackF(G(:,:,:,i),T(:,:,:,i),Mlist_ED,Fmat(:,:,i));
|
||||||
|
end
|
||||||
|
|
||||||
|
for i = 1:length(thetalist)
|
||||||
|
F_Simpack_delta(:,:,i) = F_Simpack(:,:,i)-F_Simpack_Init(:,:,i);
|
||||||
|
end
|
||||||
|
% plot Torque
|
||||||
|
% above 2020b
|
||||||
|
% F_Simpack = pagetranspose(F_Simpack);
|
||||||
|
% below 2020b
|
||||||
|
F_Simpack_delta = permute(F_Simpack_delta,[2 1 3]);
|
||||||
|
plot(thetalist(:,6),-reshape(F_Simpack_delta(7,5,:),[1,length(F_Simpack_delta)]));
|
||||||
|
% test = fileData6.data(:,(6*(8+1))+11*3-5);
|
||||||
|
% test_time = fileData6.data(:,6+1+11);
|
||||||
|
% hold on;plot(test_time,test)
|
||||||
Loading…
Reference in New Issue