%% 外力投影
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 = [2*pi;0;0;0;0;0;0;0];
theta_init = theta(1:8);
theta_delta = theta_init - theta_stow;
traj_space = [];

Vel_phi=zeros(1,1);Acc_phi=zeros(1,1);dt=1/10;phi=zeros(1,1);
Mphi = diag(5*ones(1,1));
Kphi = diag(40*ones(1,1));
Cphi = diag(0*ones(1,1));
phi_log =[];

Ft(1) = 0;

for i = 1:1000
    Acc_phi = inv(Mphi)*(Ft(i)-Kphi*Vel_phi-Cphi*Vel_phi.^2);
    Vel_phi = Vel_phi + Acc_phi*dt;
    phi = phi + Vel_phi*dt;
    if phi>=1
        phi=1;
        Acc_phi = 0;
        Vel_phi = 0;
    elseif phi<=0
        phi=0;
        Acc_phi = 0;
        Vel_phi = 0;
    end
    phi_log = [phi_log,phi];
    theta_phi(:,i) = theta_init - theta_delta .* phi;
    dtheta_phi = -theta_delta;
    
    %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;10;10;0];
%     Ft(i+1) = Fext(4:6)' * dx(:,i)/(dx(:,i)'*dx(:,i))*dx(:,i);
%     Ft(i+1) =  Fext(4:6)' *  dx(:,i)/norm(dx(:,i));
%     Ft(i+1) = sin(2*pi*dt*i);
    Ft(i+1) = 1;
end
%%
L1=Link('revolute', 'd', 0, 'a', 0, 'alpha', 0, 'offset',0,'qlim',deg2rad([-110,110]),'modified');
L2=Link('revolute', 'd', 0, 'a', 0.2, 'alpha', pi/2, 'offset',0,'qlim',deg2rad([-120,120]),'modified');
L3=Link('revolute', 'd', 0, 'a', 0.5, 'alpha', 0, 'offset',0,'qlim',deg2rad([-160,160]),'modified');
L4=Link('revolute', 'd', 0, 'a', 0.45, 'alpha', 0, 'offset',0,'qlim',deg2rad([-160,160]),'modified');
L5=Link('revolute', 'd', 0, 'a', 0.12, 'alpha', -pi/2, 'offset',-pi/2,'qlim',deg2rad([-90,90]),'modified');
L6=Link('revolute', 'd', 0.28, 'a', 0, 'alpha', -pi/2, 'offset',0,'qlim',deg2rad([-270,270]),'modified');
L7=Link('revolute', 'd', 0.40, 'a', 0, 'alpha', -pi/2, 'offset',-pi/2,'qlim',deg2rad([-270,270]),'modified');
L8=Link('revolute', 'd', 0, 'a', 0, 'alpha', -pi/2, 'offset',-pi/2,'qlim',deg2rad([-71,79]),'modified');
L9=Link('prismatic', 'a', 0.126493, 'alpha', -pi/2, 'theta',0,'offset',-(0.59295-0.2772),'qlim',[0,0.342],'modified');
DVT=SerialLink([L1 L2 L3 L4 L5 L6 L7 L8 L9],'name','DVT'); %连接连杆
% DVT.plot([0 0 0 0 0 0 0 0 0])%机械臂图
isVideoRecordEnable = 1;
view_angle1 = -136;
view_angle2 = 32;
if isVideoRecordEnable 
    myVideo = VideoWriter('DVT1_sim'); %open video file
    myVideo.FrameRate = 30;  %can adjust this, 5 - 10 works well for me
    open(myVideo);
    axis([-1 2.0 -1 1 -2 1.6]);
    view([view_angle1 view_angle2]);
end

for i = 1:1000
    if isVideoRecordEnable
        % drawing
        DVT.plot([theta_phi(:,i);0]')%机械臂图
        axis([-1 2.0 -1 1 -2 1.6]);
        hold on;
        view([view_angle1 view_angle2]);
        drawnow;
        frame = getframe(gcf); %get frame
        writeVideo(myVideo, frame);
    end
end