clc;clear;close;
fh=[0 0 0 0 -5 0]'; fa=[0 0 0 0 0 1]';
Thf=[1 0 0 -100; 0 1 0 0;0 0 1 0; 0 0 0 1]; Taf=[1 0 0 -250;0 0 1 0;0 -1 0 0;0 0 0 1]; Adjthf=[Thf(1:3,1:3) zeros(3,3);Poe_Skew_symmetric(Thf(1:3,4))*Thf(1:3,1:3), Thf(1:3,1:3)]; Adjtaf=[Taf(1:3,1:3) zeros(3,3);Poe_Skew_symmetric(Taf(1:3,4))*Taf(1:3,1:3), Taf(1:3,1:3)];
Ff=Adjthf'*fh+Adjtaf'*fa；
function [ mar ] = Poe_Skew_symmetric( Columm ) %UNTITLED3 Summary of this function goes here % Detailed explanation goes here mar=[0 -Columm(3) Columm(2);Columm(3) 0 -Columm(1);-Columm(2) Columm(1) 0]; end
