%% 外力投影
close all;clc;clear
file = [];
opt.robot_def = 'direct';
opt.KM_method = 'SCREW';
opt.Vel_method = 'Direct';
opt.LD_method = 'Direct';
opt.debug = false;
opt.robotName = 'R1000_DVT';
opt.reGenerate = false;
opt.Isreal = true;
opt.isJointTorqueSensor = true;
opt.isSixAxisFTSensor = false;
theta = zeros(9,1);
dtheta = zeros(9,1);
ddtheta = zeros(9,1);
get_robot_func = sprintf('get_robot_%s',opt.robotName);
robot = feval(get_robot_func,theta,dtheta,ddtheta,file,opt);
get_Kinematics_func = sprintf('get_Kinematics_%s',opt.robotName);
robot = feval(get_Kinematics_func,robot,opt);

% theta_stow = deg2rad([0;113;-160;47;80;90;90;0]);
theta_stow = [pi;0;0;0;0;0;0;0];
theta_delta = theta(1:8) - theta_stow;
phi = linspace(0,1,100); %?
theta_phi = theta(1:8) - theta_delta .* phi;
dtheta_phi = -theta_delta;

traj_space = [];
for i = 1:size(theta_phi,2)
%hack theta
theta = zeros(9,1);
theta(1:8) = theta_phi(:,i);
%get robot
robot = feval(get_robot_func,theta,dtheta,ddtheta,file,opt);
get_Kinematics_func = sprintf('get_Kinematics_%s',opt.robotName);
robot = feval(get_Kinematics_func,robot,opt);
%FK
traj_space(:,:,i) = robot.kine.TW(:,:,8);
Jaco = JacobianSpace(robot.slist(:,1:8),theta(1:8)); 
dtraj_space(:,i) =Jaco*dtheta_phi; %fix
dx_ = VecTose3(dtraj_space(:,i))*[traj_space(1:3,4,i);1];
dx(:,i) = dx_(1:3);
% mehod 1: only use linear force 
Fext = [0;0;0;1;1;0];
% Ft(:,i) = Fext(4:6)' * dx(:,i)/(dx(:,i)'*dx(:,i))*dx(:,i);
Ft(i) =  Fext(4:6)' *  dx(:,i)/norm(dx(:,i));
end