diff --git a/DVT1_sim.mp4 b/DVT1_sim.mp4
new file mode 100644
index 0000000..88ab6e3
Binary files /dev/null and b/DVT1_sim.mp4 differ
diff --git a/test_New_grab.m b/test_New_grab.m
new file mode 100644
index 0000000..69161e0
--- /dev/null
+++ b/test_New_grab.m
@@ -0,0 +1,196 @@
+%% 外力投影
+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/3;1.1;-2.1;0.345;0;3.14;0;0.262];
+theta_init = deg2rad([0;0;-160;47;80;90;90;0]);
+theta_init = deg2rad([0;0;100;30;80;80;90;0]);
+% theta_init = deg2rad([0;110;-160;47;80;90;90;0]);
+theta_stow = [0;1.2;-2.2;0.345;0;3.14;0;0.262];
+theta_init = [0;1.1;-2.1;0.345;0;3.14;0;0.262];
+%
+theta_stow = [1;0;0;0;0;0;0;0];
+theta_init = [0;0;0;0;0;0;0;0];
+theta_delta =  theta_stow - theta_init;
+traj_space = [];
+
+dq_max=zeros(1,1);ddq_max=zeros(1,1);dt=1/20;
+dq=zeros(8,1); 
+% Mphi = diag(5*ones(1,1));
+M = diag(10*ones(1,1));
+% Kphi = diag(40*ones(1,1));
+K = diag(40*ones(1,1));
+C = diag(0*ones(1,1));
+phi_log =[];dq_log =[];ddq_log =[];
+
+Ft(1) = 0;
+
+for i = 1:100
+    ddq_max = inv(M)*(Ft(i)-K*dq_max-C.*dq_max.^2);
+    dq_max = dq_max + ddq_max*dt;
+    dq_max = min(1, max(-1, dq_max)); % dq_lim
+    dq = (theta_delta)/(max(abs(theta_delta)))*dq_max; %joint space vel
+    dq_log = [dq_log,dq];
+    if i == 1
+        theta_J1J8(:,i) = theta_init;
+    else
+        theta_J1J8(:,i) = theta_J1J8(:,i-1)+dq*dt;
+    end
+    %saturation
+    for j = 1:length(theta_init)
+        if theta_stow(j) > theta_init(j)
+            if theta_J1J8(j,i) > theta_stow(j)
+                theta_J1J8(j,i) = theta_stow(j);
+            elseif theta_J1J8(j,i) < theta_init(j)
+                theta_J1J8(j,i) = theta_init(j);
+            end
+        else
+            if theta_J1J8(j,i) > theta_init(j)
+                theta_J1J8(j,i) = theta_init(j);
+            elseif theta_J1J8(j,i) < theta_stow(j)
+                theta_J1J8(j,i) = theta_stow(j);
+            end
+        end
+    end
+    
+    %hack theta
+    theta_J1ISA= zeros(9,1);
+    theta_J1ISA(1:8) = theta_J1J8(:,i);
+    %get robot
+    robot = feval(get_robot_func,theta_J1ISA,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(:,:,7);
+    Jaco = JacobianSpace(robot.slist(:,1:7),theta_J1ISA(1:7));
+    dtraj_space(:,i) =Jaco*dq(1:7); %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_sensor = [0;0;0;30;-40;0];
+    permutationMatrix = diag([-1,-1,1,1]);
+    Fext_base = Adjoint(permutationMatrix*inv(traj_space(:,:,i)))'*Fext_sensor;
+    %     Ft(i+1) = Fext(4:6)' * dx(:,i)/(dx(:,i)'*dx(:,i))*dx(:,i);
+%     Fext_base = [0;0;0;20;20;20];
+%     if i==1
+%         if Fext_base(4)>10
+%             Ft(i+1)=-40;
+%         elseif Fext_base(4)<-10
+%             Ft(i+1)=40;
+%         end
+%     else
+%         Ft(i+1) =  Fext_base(4:6)' *  dx(:,i)/norm(dx(:,i));
+%     end
+     Ft(i+1) = 10;
+end
+%%
+% 预先创建箭头对象（初始位置和方向可以随意设置，后续会更新）
+x_axis = quiver3(0, 0, 0, 0, 0, 0, 'r');
+x_axis.LineWidth = 3;
+x_axis.AutoScaleFactor = 0.5;
+hold on;
+test = traj_space(1:3,4,:);
+reshape(test,[3,size(test,3)]);
+for i = 1:100
+    % 绘制机械臂
+    %     DVT.plot([theta_phi(:,i);0]')
+    %     T_L8 = DVT_L8.fkine(theta_phi(:,i));
+
+    % 设置坐标轴范围
+    axis([-1 2.0 -1 1 -2 1.6]);
+    plot3(test(1,i), test(2,i), test(3,i), 'bo', 'MarkerSize', 5, 'MarkerFaceColor', 'b');
+    % 更新箭头的位置和方向（而不是重新创建）
+    set(x_axis, 'XData', test(1,i), 'YData', test(2,i), 'ZData', test(3,i), ...
+                'UData', dx(1,i)/norm(dx(:,i)), 'VData', dx(2,i)/norm(dx(:,i)), 'WData', dx(3,i)/norm(dx(:,i)));
+    % 设置视角
+%     view([view_angle1 view_angle2]);
+    % 刷新图形
+    drawnow;
+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'); %连接连杆
+% 创建仅包含前8个连杆的子机器人
+links_before_L9 = DVT.links(1:8);  % 提取L1-L8
+DVT_L8 = SerialLink(links_before_L9, 'name', 'DVT_up_to_L8'); 
+% DVT.plot([0 0 0 0 0 0 0 0 0])%机械臂图
+% 预先计算所有轨迹点
+traj_points = zeros(3, 100);  % 假设总共有100个点
+for i = 1:100
+    T_L8 = DVT_L8.fkine(theta_J1J8(:,i)); 
+    traj_points(:,i) = T_L8.t;  % 存储所有轨迹点
+end
+
+% 图形初始化
+figure;
+axis([-1 2.0 -1 2 -2 1.6]);
+view_angle1 = -156;view_angle2=46;
+view([view_angle1 view_angle2]);
+isVideoRecordEnable = 1;
+hold on;
+grid on;
+
+% 一次性绘制全部轨迹点
+plot3(traj_points(1,:), traj_points(2,:), traj_points(3,:),...
+      'b-o', 'MarkerSize', 4, 'MarkerFaceColor', 'b', 'LineWidth', 1.5);
+
+% 初始化箭头
+x_axis = quiver3(0,0,0,0,0,0, 'r');
+x_axis.LineWidth = 3;
+x_axis.AutoScaleFactor = 0.5;
+
+% 视频录制设置
+if isVideoRecordEnable
+    myVideo = VideoWriter('DVT1_sim', 'MPEG-4');
+    myVideo.FrameRate = 30;
+    open(myVideo);
+end
+% 主循环（只更新机械臂和箭头）
+for i = 1:100
+    % 绘制机械臂（保留轨迹不擦除）
+    DVT.plot([theta_J1J8(:,i);0]');
+    
+    % 更新箭头
+    set(x_axis, 'XData',traj_points(1,i), 'YData',traj_points(2,i), 'ZData',traj_points(3,i),...
+                'UData',dx(1,i)/norm(dx(:,i)), 'VData',dx(2,i)/norm(dx(:,i)), 'WData',dx(3,i)/norm(dx(:,i)));
+%     view([view_angle1 view_angle2]);
+    % 录制视频
+    if isVideoRecordEnable
+        frame = getframe(gcf);
+        writeVideo(myVideo, frame);
+    end
+    
+    drawnow limitrate;  % 更高效的刷新方式
+end
+
+% 确保关闭视频文件
+if isVideoRecordEnable
+    close(myVideo);
+    disp('视频录制完成');
+end
\ No newline at end of file
diff --git a/test_grab2.m b/test_grab2.m
index fd89ce8..94875b4 100644
--- a/test_grab2.m
+++ b/test_grab2.m
@@ -24,6 +24,8 @@ theta_stow = deg2rad([0;113;-160;47;80;90;90;0]);
 % theta_init = deg2rad([0;0;-160;47;80;90;90;0]);
 theta_init = deg2rad([0;110;-160;47;80;90;90;0]);
 % theta_init = [0;1.1;-2.1;0.345;0;3.14;0;0.262];
+theta_stow = [0;1.972;-2.79;0.82;1.396;1.57;1.57;0];
+theta_init = [0;1.1;-2.1;0.345;0;3.14;0;0.262];
 theta_delta = theta_init - theta_stow;
 traj_space = [];
 
@@ -71,13 +73,13 @@ for i = 1:1000
     dx_ = VecTose3(dtraj_space(:,i))*[traj_space(1:3,4,i);1];
     dx(:,i) = dx_(1:3);
     % mehod 1: only use linear force
-%     Fext_sensor = [0;0;0;30;-40;0]; 
-%     permutationMatrix = diag([-1,-1,1,1]);
-%     Fext_base = Adjoint(permutationMatrix*inv(traj_space(:,:,i)))'*Fext_sensor;
+    Fext_sensor = [0;0;0;30;-40;0]; 
+    permutationMatrix = diag([-1,-1,1,1]);
+    Fext_base = Adjoint(permutationMatrix*inv(traj_space(:,:,i)))'*Fext_sensor;
 %     Ft(i+1) = Fext(4:6)' * dx(:,i)/(dx(:,i)'*dx(:,i))*dx(:,i);
-%     Ft(i+1) =  Fext_base(4:6)' *  dx(:,i)/norm(dx(:,i));
+    Ft(i+1) =  Fext_base(4:6)' *  dx(:,i)/norm(dx(:,i));
 %     Ft(i+1) = sin(2*pi*dt*i);
-    Ft(i+1) = 10;
+%     Ft(i+1) = 10;
 end
 %%
 % 预先创建箭头对象（初始位置和方向可以随意设置，后续会更新）