18 lines
615 B
Mathematica
18 lines
615 B
Mathematica
|
function Flist = getSimpackF(T_GC,T_O,Mlist,Fmat)
|
||
|
|
N=size(Fmat,1);
|
||
|
|
Flist = zeros(6,N);
|
||
|
|
|
||
|
|
%base frame
|
||
|
|
% Adgab = Adjoint(TransInv(T_GC(:,:,1))*Mlist(:,:,1));
|
||
|
|
% Flist(:,1) = Adgab'*Fmat(1,:)';
|
||
|
|
% for i = 2: N
|
||
|
|
% % Adgab = Adjoint(TransInv(T_GC(:,:,i))*T_O(:,:,i-1)*Mlist(:,:,i));
|
||
|
|
% Adgab = Adjoint(TransInv(T_GC(:,:,i))*T_O(:,:,i));
|
||
|
|
% Flist(:,i) = Adgab'*Fmat(i,:)';
|
||
|
|
% endAdgab = Adjoint(TransInv(T_GC(:,:,1))*Mlist(:,:,1));
|
||
|
|
for i = 1: N
|
||
|
|
% Adgab = Adjoint(TransInv(T_GC(:,:,i))*T_O(:,:,i-1)*Mlist(:,:,i));
|
||
|
|
Adgab = Adjoint(TransInv(T_GC(:,:,i))*T_O(:,:,i));
|
||
|
|
Flist(:,i) = Adgab'*Fmat(i,:)';
|
||
|
|
end
|
||
|
|
end
|