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 = [0;113;-160;47;80;90;90;0];
theta_delta = theta - theta_stow;
phi = linspace(0,1,100)*ones(1,6); %?
theta_phi = theta - theta_delta * phi;
dtheta_phi = -phi;

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);
traj_space = [];
traj_space = [traj_space;robot.TW];
dtraj_space = jaco*dtheta_phi; %fix
%% 外力估计
t=linspace(0,100,100);
% Fext = 100*sin(2*pi*t);
Fext = 10;
% Ft = Fext*dtraj_space; %fix
% % saturation and switch
%% 阻抗控制
Vel_phi=zeros(1,1);Acc_phi=zeros(1,1);dt=1/2000;phi=zeros(1,1);
Mphi = diag(10*ones(1,1));
Kphi = diag(80*ones(1,1));
Cphi = diag(0*ones(1,1));
phi_log =[];
for i = 1:10000
Acc_phi = inv(Mphi)*(Fext-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;
end
phi_log = [phi_log,phi];
end
%% 关节运动
theta = zeros(8,1);
theta_stow = deg2rad([0;113;-160;47;80;90;90;0]);
theta_delta = theta - theta_stow;
theta_phi = theta - theta_delta * phi_log;