diff --git a/R1000_Dynamics_num_traj.m b/R1000_Dynamics_num_traj.m
new file mode 100644
index 0000000..80856cf
--- /dev/null
+++ b/R1000_Dynamics_num_traj.m
@@ -0,0 +1,259 @@
+%% R1000
+N=9;
+% traj
+load('pose1_isa_depth_95mm.mat');
+time = (100:size(q_log,2))/2000;
+q_J = q_log(2:10,100:end);
+qd_J = vel_log(2:10,100:end);
+qdd_J = acc_log(2:10,100:end);
+
+% Dynamics parameters
+link_mass = robot.m;
+com_pos = robot.com;
+link_inertia = robot.I;
+
+thetalist = q_J';
+dthetalist = qd_J';
+ddthetalist = qdd_J';
+
+%real traj
+% get_GCTraj_R1000_DVT;
+% thetalist = idntfcnTrjctry(6).q';
+% dthetalist = idntfcnTrjctry(6).qd';
+% ddthetalist = idntfcnTrjctry(6).qdd';
+
+% Get general mass matrix
+Glist=[];
+for i = 1:N
+    link_inertia(:,:,i) = robot.Home.R(:,:,i)'*link_inertia(:,:,i)*robot.Home.R(:,:,i);
+    Gb= [link_inertia(:,:,i),zeros(3,3);zeros(3,3),link_mass(i)*diag([1,1,1])];
+    Glist = cat(3, Glist, Gb);
+end
+
+% Get the com pos transformation in each joint reference frame
+% Mlist_CG = [];
+% for i = 0:N-1
+%     if i == 0
+%         M = robot.T(:,:,i+1)*transl(com_pos(:,i+1));
+%     else
+%         M = TransInv(transl(com_pos(:,i)))*robot.T(:,:,i+1)*transl(com_pos(:,i+1));
+%     end
+%     Mlist_CG = cat(3, Mlist_CG, M);
+% end
+% M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
+% Mlist_CG = cat(3, Mlist_CG, M);
+
+% Get the com pos transformation in each joint reference frame
+% FIXME: BUG here
+% Mlist_CG=[];
+% for i = 0:N-1
+%     if i == 0
+%         M=robot.T(:,:,i+1)*transl(com_pos(:,i+1));
+%     else
+%         rotation_i = diag([1,1,1]);
+%         for j = 1:i
+%             rotation_i = rotation_i*TransToRp(robot.T(:,:,i));
+%             rotation_j = rotation_i*TransToRp(robot.T(:,:,i+1));
+%         end
+%         M = TransInv(RpToTrans(rotation_i,rotation_i*com_pos(:,i)))*robot.T(:,:,i+1)*RpToTrans(rotation_j,rotation_j*com_pos(:,i+1));
+%     end
+%     Mlist_CG = cat(3, Mlist_CG, M);
+% end
+% M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
+% Mlist_CG = cat(3, Mlist_CG, M);
+
+% ct=[];
+% Mlist_CG=[];
+% for i = 1:N
+%     if i == 1
+%         ct(:,i) = com_pos_R1(:,i);
+%     elseif i< 9
+%         ct(:,i) = -com_pos_R2(:,i-1)+com_pos_R1(:,i);
+%     else
+%         ct(:,i) = -com_pos_R1(:,i-1)-[0;0;0.05896]+com_pos_R1(:,i);
+%     end
+%     robot.Home.com(:,i) = ct(:,i);
+%     M = RpToTrans(robot.T(1:3,1:3,i),robot.Home.R(:,:,i)*robot.Home.com(:,i));
+%     Mlist_CG = cat(3, Mlist_CG, M);
+% end
+% M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
+% Mlist_CG = cat(3, Mlist_CG, M);
+
+% Mlist_CG=[];
+% for i = 1:N
+%         if i == 1
+%         M = [diag([1,1,1]),com_pos_R1(:,i);zeros(1,3),1];
+%     elseif i<=8
+%         M = RpToTrans(TransToRp(robot.T(:,:,i)),robot.Home.R(:,:,i)*(-com_pos_R2(:,i-1)+com_pos_R1(:,i)));
+%     elseif i==9
+%         M = RpToTrans(TransToRp(robot.T(:,:,i)),robot.Home.R(:,:,i)*(-[0;0;0.05896]+com_pos_R1(:,i-1)+com_pos_R1(:,i)));
+%         end
+%         Mlist_CG = cat(3, Mlist_CG, M);
+% end
+% M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
+% Mlist_CG = cat(3, Mlist_CG, M);
+
+% get the CG at the world base frame
+com_pos_R1 = robot.com_pos_R1;
+com_pos_R2 = robot.com_pos_R2;
+ct=[];
+Mlist_CG_Base=[];
+for i = 1:N
+    if i == 1
+        ct(:,i) = com_pos_R1(:,i);
+    elseif i< 9
+        ct(:,i) = ct(:,i-1)-com_pos_R2(:,i-1)+com_pos_R1(:,i);
+    else
+        ct(:,i) = ct(:,i-1)-com_pos_R1(:,i-1)-[0.23;0;0.05896]+com_pos_R1(:,i);
+    end
+    robot.Home.com(:,i) = ct(:,i);
+    M_CG_Base = RpToTrans(robot.Home.R(:,:,i),robot.Home.com(:,i));
+    Mlist_CG_Base = cat(3, Mlist_CG_Base, M_CG_Base);
+end
+% get the CG at the last GC frame
+Mlist_CG=[];
+for i = 1:N
+    if i == 1
+        Mlist_CG(:,:,i) = Mlist_CG_Base(:,:,i);
+    else
+        Mlist_CG(:,:,i) = TransInv(Mlist_CG_Base(:,:,i-1))*Mlist_CG_Base(:,:,i);
+    end
+end
+M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
+Mlist_CG = cat(3, Mlist_CG, M);
+
+
+% Get the end efforce transformation in each joint reference frame
+Mlist_ED = [];
+for i = 1:N
+    M = robot.T(:,:,i);
+    Mlist_ED = cat(3, Mlist_ED, M);
+end
+M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
+Mlist_ED = cat(3, Mlist_ED, M);
+
+%TODO: Get Slist form DH table method
+% RRRRRRRRP
+Slist=robot.slist;
+exf=[0;0;0;0;1;0];
+
+for i = 1:length(thetalist)
+    [V1(:,:, i),Vd1(:,:, i),Adgab_mat(:,:,:,i),Fmat(:,:,i),taumat(:,i)] ...
+        = InverseDynamics_debug(thetalist(i,:)', dthetalist(i,:)', ddthetalist(i,:)', ...
+        [0;0;-9.806], exf, Mlist_CG, Glist, Slist);
+    %         [0;0;-9.806], exf, Mlist_CG, Glist, Slist);
+
+    G(:,:,:,i) = FKinSpaceExpand(Mlist_CG, Slist, thetalist(i,:)');
+    T(:,:,:,i)=FKinSpaceExpand(Mlist_ED, Slist, thetalist(i,:)');
+    %Want to get the result from TC_delta, which means F at CG represent under frame at the last origin  
+    %why we need Mlist_ED
+    %please explain this more
+    F_Simpack(:,:,i) = getSimpackF(G(:,:,:,i),T(:,:,:,i),Mlist_ED,Fmat(:,:,i));
+end
+% plot Torque
+% above 2020b 
+% F_Simpack = pagetranspose(F_Simpack);
+% below 2020b
+
+% for i = 1:3
+%     subplot(3,1,i);
+%     hold on;
+%     %added minus, so should be the same as simpack
+%     plot(time,taumat(i+6,:))
+%     xlabel('time(s)')
+%     ylabel('Torque(Nm)')
+% %     plot(SPCK_Result.Crv(i+4).x,SPCK_Result.Crv(i+4).y)
+% end
+
+F_Simpack = permute(F_Simpack,[2 1 3]);
+F_Simpack = -F_Simpack;
+
+% 确保输出文件夹存在
+output_main = 'output';
+output_sub = fullfile(output_main, 'pose1_isa_depth_95mm');
+
+if ~exist(output_main, 'dir')
+    mkdir(output_main);
+end
+if ~exist(output_sub, 'dir')
+    mkdir(output_sub);
+end
+
+% 获取时间序列长度
+n_time = size(F_Simpack, 3);
+
+% 遍历所有关节（1到8）
+for joint_idx = 1:8
+    fig = figure('Position', [100, 100, 1000, 900]);  % 创建大尺寸图形窗口
+    
+    % 遍历三个力矩分量
+    for comp_idx = 1:3
+        subplot(3, 1, comp_idx);
+        hold on;
+        
+        % 提取并处理数据（取负值以匹配Simpack约定）
+        data = -squeeze(F_Simpack(joint_idx, comp_idx, :));
+        
+        % 绘制力矩曲线
+        plot(time, data, 'LineWidth', 1.5);
+        
+        % 计算并标注最大绝对值（峰值）
+        [abs_max, max_idx] = max(abs(data));  % 找到最大绝对值
+        peak_value = data(max_idx);           % 对应的实际值（带符号）
+        
+        % 标记峰值点
+        plot(time(max_idx), peak_value, 'ro', 'MarkerSize', 8, 'LineWidth', 2);
+        
+        % 创建标注文本
+        text_str = sprintf('Peak: %.2f Nm (|%.2f|)', peak_value, abs_max);
+        
+        % 确定文本位置（避免重叠）
+        y_range = ylim;
+        y_offset = 0.05 * (y_range(2) - y_range(1));
+        
+        if peak_value >= 0
+            text_pos = [time(max_idx), peak_value - y_offset];
+            vert_align = 'top';
+        else
+            text_pos = [time(max_idx), peak_value + y_offset];
+            vert_align = 'bottom';
+        end
+        
+        % 添加文本标注
+        text(text_pos(1), text_pos(2), text_str, ...
+            'VerticalAlignment', vert_align, 'HorizontalAlignment', 'center', ...
+            'FontSize', 10, 'BackgroundColor', 'white', 'EdgeColor', 'black');
+        
+        % 添加标签和网格
+        switch comp_idx
+            case 1
+                title('Bending Moment (M_x)');
+            case 2
+                title('Bending Moment (M_y)');
+            case 3
+                title('Torque (M_z)');
+        end
+        xlabel('Time [s]');
+        ylabel('Moment [Nm]');
+        grid on;
+        box on;
+    end
+    
+    % 添加总标题
+    sgtitle(sprintf('Joint %d - Moment Components (Peak Values)', joint_idx), ...
+           'FontSize', 14, 'FontWeight', 'bold');
+    
+    % 保存图形
+    saveas(fig, fullfile(output_sub, sprintf('joint_%d.png', joint_idx)));
+    close(fig);  % 关闭图形释放内存
+end
+
+disp('所有关节图表已保存至 output/pose2_isa_depth_95mm 文件夹');
+
+% Use Body Twist cal linear vel, but can't cal the end frame vel
+% [V2] = InverseDynamics_sym(thetalist, dthetalist, ddthetalist, ...
+%     [0;0;0], exf, Mlist, Glist, Slist);
+% j=1;
+% Vlinear(:, j+1) = BodyVelToLinearVel(V2(:,j+1),G(:,:,j)*M12);
+% j=2;
+% Vlinear(:, j+1) = BodyVelToLinearVel(V2(:,j+1),G(:,:,j)*M23);
\ No newline at end of file
diff --git a/getTorqueData.m b/getTorqueData.m
new file mode 100644
index 0000000..131590b
--- /dev/null
+++ b/getTorqueData.m
@@ -0,0 +1,74 @@
+% 加载数据文件
+load('idntfcnTrjctryCell_full2.mat');
+
+% 获取数据矩阵
+data = idntfcnTrjctryCell{7,1};
+[num_samples, ~] = size(data);
+
+% 定义关节列索引
+joint_cols = 105:3:126; % J1到J8的列索引 (105,108,111,...,126)
+
+% 创建时间向量（采样频率2000 Hz）
+t = (0:num_samples-1)' / 2000; % 单位为秒
+
+% 循环处理每个关节
+for joint_idx = 1:8
+    % 创建新图形窗口
+    fig = figure('Position', [100, 100, 800, 500], 'Color', 'white');
+    
+    % 获取当前关节的数据
+    col = joint_cols(joint_idx);
+    if joint_idx == 8
+        torque = data(:, joint_cols(joint_idx-1)+2);
+    else
+        torque = data(:, joint_cols(joint_idx));
+    end
+    % 计算最大绝对扭矩值及其位置
+    [max_abs, max_idx] = max(abs(torque));
+    max_time = t(max_idx);
+    max_value = torque(max_idx);
+    
+    % 绘制力矩曲线
+    plot(t, torque, 'b-', 'LineWidth', 1.5);
+    
+    % 标记最大绝对值点
+    hold on;
+    plot(max_time, max_value, 'ro', 'MarkerSize', 10, 'LineWidth', 2);
+    
+    % 在右上角添加标注文本
+    annotation_text = sprintf('Max Abs Torque: %.3f Nm\\nAt Time: %.3f s', max_abs, max_time);
+    text(0.98, 0.98, annotation_text, ...
+        'Units', 'normalized', ...
+        'VerticalAlignment', 'top', ...
+        'HorizontalAlignment', 'right', ...
+        'FontSize', 12, ...
+        'BackgroundColor', [1 1 1 0.7], ...
+        'Margin', 5, ...
+        'EdgeColor', 'k');
+    
+    % 设置图形属性
+    title(sprintf('J%d Torque vs Time', joint_idx), 'FontSize', 16, 'FontWeight', 'bold');
+    xlabel('Time (s)', 'FontSize', 14);
+    ylabel('Torque (Nm)', 'FontSize', 14);
+    grid on;
+    
+    % 设置坐标轴范围
+%     xlim([min(t), max(t)]);
+%     y_range = max_abs * 1.3; % 基于最大绝对值设置Y轴范围
+%     ylim([-y_range, y_range]);
+    
+    % 添加采样率信息
+    text(0.98, 0.02, 'Sampling Rate: 2000 Hz', ...
+        'Units', 'normalized', ...
+        'FontSize', 10, ...
+        'HorizontalAlignment', 'right', ...
+        'BackgroundColor', [1 1 1 0.5]);
+    
+    % 保存图形为PNG文件
+    filename = sprintf('Torque_joint_%d.png', joint_idx);
+    saveas(fig, filename);
+    fprintf('Saved: %s\n', filename);
+    
+    % 关闭图形
+    close(fig);
+end
\ No newline at end of file
diff --git a/output/pos1_pitch_2rad/joint_1.png b/output/pos1_pitch_2rad/joint_1.png
new file mode 100644
index 0000000..cdb6774
Binary files /dev/null and b/output/pos1_pitch_2rad/joint_1.png differ
diff --git a/output/pos1_pitch_2rad/joint_2.png b/output/pos1_pitch_2rad/joint_2.png
new file mode 100644
index 0000000..dc294ec
Binary files /dev/null and b/output/pos1_pitch_2rad/joint_2.png differ
diff --git a/output/pos1_pitch_2rad/joint_3.png b/output/pos1_pitch_2rad/joint_3.png
new file mode 100644
index 0000000..df2dc72
Binary files /dev/null and b/output/pos1_pitch_2rad/joint_3.png differ
diff --git a/output/pos1_pitch_2rad/joint_4.png b/output/pos1_pitch_2rad/joint_4.png
new file mode 100644
index 0000000..ebd9e7f
Binary files /dev/null and b/output/pos1_pitch_2rad/joint_4.png differ
diff --git a/output/pos1_pitch_2rad/joint_5.png b/output/pos1_pitch_2rad/joint_5.png
new file mode 100644
index 0000000..437a185
Binary files /dev/null and b/output/pos1_pitch_2rad/joint_5.png differ
diff --git a/output/pos1_pitch_2rad/joint_6.png b/output/pos1_pitch_2rad/joint_6.png
new file mode 100644
index 0000000..58406dd
Binary files /dev/null and b/output/pos1_pitch_2rad/joint_6.png differ
diff --git a/output/pos1_pitch_2rad/joint_7.png b/output/pos1_pitch_2rad/joint_7.png
new file mode 100644
index 0000000..7240e9b
Binary files /dev/null and b/output/pos1_pitch_2rad/joint_7.png differ
diff --git a/output/pos1_pitch_2rad/joint_8.png b/output/pos1_pitch_2rad/joint_8.png
new file mode 100644
index 0000000..a8b4f77
Binary files /dev/null and b/output/pos1_pitch_2rad/joint_8.png differ
diff --git a/output/pos1_yaw_2rad/joint_1.png b/output/pos1_yaw_2rad/joint_1.png
new file mode 100644
index 0000000..8bb78cc
Binary files /dev/null and b/output/pos1_yaw_2rad/joint_1.png differ
diff --git a/output/pos1_yaw_2rad/joint_2.png b/output/pos1_yaw_2rad/joint_2.png
new file mode 100644
index 0000000..1be8785
Binary files /dev/null and b/output/pos1_yaw_2rad/joint_2.png differ
diff --git a/output/pos1_yaw_2rad/joint_3.png b/output/pos1_yaw_2rad/joint_3.png
new file mode 100644
index 0000000..7577837
Binary files /dev/null and b/output/pos1_yaw_2rad/joint_3.png differ
diff --git a/output/pos1_yaw_2rad/joint_4.png b/output/pos1_yaw_2rad/joint_4.png
new file mode 100644
index 0000000..009094c
Binary files /dev/null and b/output/pos1_yaw_2rad/joint_4.png differ
diff --git a/output/pos1_yaw_2rad/joint_5.png b/output/pos1_yaw_2rad/joint_5.png
new file mode 100644
index 0000000..5e0156b
Binary files /dev/null and b/output/pos1_yaw_2rad/joint_5.png differ
diff --git a/output/pos1_yaw_2rad/joint_6.png b/output/pos1_yaw_2rad/joint_6.png
new file mode 100644
index 0000000..9ade1f3
Binary files /dev/null and b/output/pos1_yaw_2rad/joint_6.png differ
diff --git a/output/pos1_yaw_2rad/joint_7.png b/output/pos1_yaw_2rad/joint_7.png
new file mode 100644
index 0000000..ec89230
Binary files /dev/null and b/output/pos1_yaw_2rad/joint_7.png differ
diff --git a/output/pos1_yaw_2rad/joint_8.png b/output/pos1_yaw_2rad/joint_8.png
new file mode 100644
index 0000000..3689640
Binary files /dev/null and b/output/pos1_yaw_2rad/joint_8.png differ
diff --git a/output/pos2_pitch_2rad/joint_1.png b/output/pos2_pitch_2rad/joint_1.png
new file mode 100644
index 0000000..c86d218
Binary files /dev/null and b/output/pos2_pitch_2rad/joint_1.png differ
diff --git a/output/pos2_pitch_2rad/joint_2.png b/output/pos2_pitch_2rad/joint_2.png
new file mode 100644
index 0000000..72092c3
Binary files /dev/null and b/output/pos2_pitch_2rad/joint_2.png differ
diff --git a/output/pos2_pitch_2rad/joint_3.png b/output/pos2_pitch_2rad/joint_3.png
new file mode 100644
index 0000000..17577aa
Binary files /dev/null and b/output/pos2_pitch_2rad/joint_3.png differ
diff --git a/output/pos2_pitch_2rad/joint_4.png b/output/pos2_pitch_2rad/joint_4.png
new file mode 100644
index 0000000..5ddb44a
Binary files /dev/null and b/output/pos2_pitch_2rad/joint_4.png differ
diff --git a/output/pos2_pitch_2rad/joint_5.png b/output/pos2_pitch_2rad/joint_5.png
new file mode 100644
index 0000000..e758f35
Binary files /dev/null and b/output/pos2_pitch_2rad/joint_5.png differ
diff --git a/output/pos2_pitch_2rad/joint_6.png b/output/pos2_pitch_2rad/joint_6.png
new file mode 100644
index 0000000..e86480e
Binary files /dev/null and b/output/pos2_pitch_2rad/joint_6.png differ
diff --git a/output/pos2_pitch_2rad/joint_7.png b/output/pos2_pitch_2rad/joint_7.png
new file mode 100644
index 0000000..1b56a2a
Binary files /dev/null and b/output/pos2_pitch_2rad/joint_7.png differ
diff --git a/output/pos2_pitch_2rad/joint_8.png b/output/pos2_pitch_2rad/joint_8.png
new file mode 100644
index 0000000..2494b4a
Binary files /dev/null and b/output/pos2_pitch_2rad/joint_8.png differ
diff --git a/output/pos2_yaw_2rad/joint_1.png b/output/pos2_yaw_2rad/joint_1.png
new file mode 100644
index 0000000..c6fbfea
Binary files /dev/null and b/output/pos2_yaw_2rad/joint_1.png differ
diff --git a/output/pos2_yaw_2rad/joint_2.png b/output/pos2_yaw_2rad/joint_2.png
new file mode 100644
index 0000000..5d8e960
Binary files /dev/null and b/output/pos2_yaw_2rad/joint_2.png differ
diff --git a/output/pos2_yaw_2rad/joint_3.png b/output/pos2_yaw_2rad/joint_3.png
new file mode 100644
index 0000000..22e737b
Binary files /dev/null and b/output/pos2_yaw_2rad/joint_3.png differ
diff --git a/output/pos2_yaw_2rad/joint_4.png b/output/pos2_yaw_2rad/joint_4.png
new file mode 100644
index 0000000..cf054de
Binary files /dev/null and b/output/pos2_yaw_2rad/joint_4.png differ
diff --git a/output/pos2_yaw_2rad/joint_5.png b/output/pos2_yaw_2rad/joint_5.png
new file mode 100644
index 0000000..e8268a3
Binary files /dev/null and b/output/pos2_yaw_2rad/joint_5.png differ
diff --git a/output/pos2_yaw_2rad/joint_6.png b/output/pos2_yaw_2rad/joint_6.png
new file mode 100644
index 0000000..5dfb97a
Binary files /dev/null and b/output/pos2_yaw_2rad/joint_6.png differ
diff --git a/output/pos2_yaw_2rad/joint_7.png b/output/pos2_yaw_2rad/joint_7.png
new file mode 100644
index 0000000..0f36f45
Binary files /dev/null and b/output/pos2_yaw_2rad/joint_7.png differ
diff --git a/output/pos2_yaw_2rad/joint_8.png b/output/pos2_yaw_2rad/joint_8.png
new file mode 100644
index 0000000..41222b3
Binary files /dev/null and b/output/pos2_yaw_2rad/joint_8.png differ
diff --git a/output/pose1_isa_depth_95mm/joint_1.png b/output/pose1_isa_depth_95mm/joint_1.png
new file mode 100644
index 0000000..9e595ec
Binary files /dev/null and b/output/pose1_isa_depth_95mm/joint_1.png differ
diff --git a/output/pose1_isa_depth_95mm/joint_2.png b/output/pose1_isa_depth_95mm/joint_2.png
new file mode 100644
index 0000000..6cb709b
Binary files /dev/null and b/output/pose1_isa_depth_95mm/joint_2.png differ
diff --git a/output/pose1_isa_depth_95mm/joint_3.png b/output/pose1_isa_depth_95mm/joint_3.png
new file mode 100644
index 0000000..5921e03
Binary files /dev/null and b/output/pose1_isa_depth_95mm/joint_3.png differ
diff --git a/output/pose1_isa_depth_95mm/joint_4.png b/output/pose1_isa_depth_95mm/joint_4.png
new file mode 100644
index 0000000..6cd4fa8
Binary files /dev/null and b/output/pose1_isa_depth_95mm/joint_4.png differ
diff --git a/output/pose1_isa_depth_95mm/joint_5.png b/output/pose1_isa_depth_95mm/joint_5.png
new file mode 100644
index 0000000..3937da2
Binary files /dev/null and b/output/pose1_isa_depth_95mm/joint_5.png differ
diff --git a/output/pose1_isa_depth_95mm/joint_6.png b/output/pose1_isa_depth_95mm/joint_6.png
new file mode 100644
index 0000000..38e6a26
Binary files /dev/null and b/output/pose1_isa_depth_95mm/joint_6.png differ
diff --git a/output/pose1_isa_depth_95mm/joint_7.png b/output/pose1_isa_depth_95mm/joint_7.png
new file mode 100644
index 0000000..41fc349
Binary files /dev/null and b/output/pose1_isa_depth_95mm/joint_7.png differ
diff --git a/output/pose1_isa_depth_95mm/joint_8.png b/output/pose1_isa_depth_95mm/joint_8.png
new file mode 100644
index 0000000..e669361
Binary files /dev/null and b/output/pose1_isa_depth_95mm/joint_8.png differ
diff --git a/output/pose2_isa_depth_95mm/joint_1.png b/output/pose2_isa_depth_95mm/joint_1.png
new file mode 100644
index 0000000..3e6f4b9
Binary files /dev/null and b/output/pose2_isa_depth_95mm/joint_1.png differ
diff --git a/output/pose2_isa_depth_95mm/joint_2.png b/output/pose2_isa_depth_95mm/joint_2.png
new file mode 100644
index 0000000..d1b537a
Binary files /dev/null and b/output/pose2_isa_depth_95mm/joint_2.png differ
diff --git a/output/pose2_isa_depth_95mm/joint_3.png b/output/pose2_isa_depth_95mm/joint_3.png
new file mode 100644
index 0000000..c187fd4
Binary files /dev/null and b/output/pose2_isa_depth_95mm/joint_3.png differ
diff --git a/output/pose2_isa_depth_95mm/joint_4.png b/output/pose2_isa_depth_95mm/joint_4.png
new file mode 100644
index 0000000..da5febb
Binary files /dev/null and b/output/pose2_isa_depth_95mm/joint_4.png differ
diff --git a/output/pose2_isa_depth_95mm/joint_5.png b/output/pose2_isa_depth_95mm/joint_5.png
new file mode 100644
index 0000000..50144a8
Binary files /dev/null and b/output/pose2_isa_depth_95mm/joint_5.png differ
diff --git a/output/pose2_isa_depth_95mm/joint_6.png b/output/pose2_isa_depth_95mm/joint_6.png
new file mode 100644
index 0000000..93c0411
Binary files /dev/null and b/output/pose2_isa_depth_95mm/joint_6.png differ
diff --git a/output/pose2_isa_depth_95mm/joint_7.png b/output/pose2_isa_depth_95mm/joint_7.png
new file mode 100644
index 0000000..7b5935c
Binary files /dev/null and b/output/pose2_isa_depth_95mm/joint_7.png differ
diff --git a/output/pose2_isa_depth_95mm/joint_8.png b/output/pose2_isa_depth_95mm/joint_8.png
new file mode 100644
index 0000000..0273bde
Binary files /dev/null and b/output/pose2_isa_depth_95mm/joint_8.png differ
diff --git a/pos1_pitch_2rad.mat b/pos1_pitch_2rad.mat
new file mode 100644
index 0000000..1797d3b
Binary files /dev/null and b/pos1_pitch_2rad.mat differ
diff --git a/pos1_pitch_2rad_15acc.mat b/pos1_pitch_2rad_15acc.mat
new file mode 100644
index 0000000..28dbe1a
Binary files /dev/null and b/pos1_pitch_2rad_15acc.mat differ
diff --git a/pos1_yaw_2rad.mat b/pos1_yaw_2rad.mat
new file mode 100644
index 0000000..7b6f834
Binary files /dev/null and b/pos1_yaw_2rad.mat differ
diff --git a/pos2_pitch_2rad.mat b/pos2_pitch_2rad.mat
new file mode 100644
index 0000000..d376131
Binary files /dev/null and b/pos2_pitch_2rad.mat differ
diff --git a/pos2_yaw_2rad.mat b/pos2_yaw_2rad.mat
new file mode 100644
index 0000000..ec0b438
Binary files /dev/null and b/pos2_yaw_2rad.mat differ
diff --git a/pose1_isa_depth_95mm.mat b/pose1_isa_depth_95mm.mat
new file mode 100644
index 0000000..fce9b88
Binary files /dev/null and b/pose1_isa_depth_95mm.mat differ
diff --git a/pose2_isa_depth_95mm.mat b/pose2_isa_depth_95mm.mat
new file mode 100644
index 0000000..a33eb6e
Binary files /dev/null and b/pose2_isa_depth_95mm.mat differ
diff --git a/test_New_grab.m b/test_New_grab.m
index 69161e0..bb3327d 100644
--- a/test_New_grab.m
+++ b/test_New_grab.m
@@ -24,27 +24,28 @@ 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;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 = [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_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_max=zeros(1,1);ddq_max=zeros(1,1);dt=1/2000;
 dq=zeros(8,1); 
 % Mphi = diag(5*ones(1,1));
-M = diag(10*ones(1,1));
+M = diag(0.005*ones(1,1));
 % Kphi = diag(40*ones(1,1));
-K = diag(40*ones(1,1));
+K = diag(0.01*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);
+for i = 1:1000
+    ddq_max = inv(M)*(Ft(i)-K*dq_max-C.*dq_max.^2)
+    ddq_max = min(1, max(-1, ddq_max)); % dq_lim
     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
@@ -90,16 +91,20 @@ for i = 1:100
     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;
+    if i==1
+        if Fext_base(4)>10
+            Ft(i+1)=-10;
+        elseif Fext_base(4)<-10
+            Ft(i+1)=10;
+        end
+    else
+        if(norm(dx(:,i))<0.001)
+            Ft(i+1) = 0;
+        else
+        Ft(i+1) =  Fext_base(4:6)' *  dx(:,i)/norm(dx(:,i));
+        end
+    end
+%      Ft(i+1) = 10;
 end
 %%
 % 预先创建箭头对象（初始位置和方向可以随意设置，后续会更新）
diff --git a/test_New_grab2.m b/test_New_grab2.m
new file mode 100644
index 0000000..312af76
--- /dev/null
+++ b/test_New_grab2.m
@@ -0,0 +1,188 @@
+%% 外力投影
+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/2000;
+dq=zeros(8,1); 
+% Mphi = diag(5*ones(1,1));
+M = diag(5*ones(1,1));
+% Kphi = diag(40*ones(1,1));
+K = diag(1*ones(1,1));
+C = diag(0*ones(1,1));
+phi_log =[];dq_log =[];ddq_log =[];
+
+Ft(1) = 0;
+
+for i = 1:10000
+    ddq_max = inv(M)*(Ft(i)-K*dq_max-C.*dq_max.^2);
+%     ddq_max = min(1, max(-1, ddq_max)); % dq_lim
+    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];
+     Ft(i+1) = -Fext_base(4);
+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_New_grab3.m b/test_New_grab3.m
new file mode 100644
index 0000000..490c95c
--- /dev/null
+++ b/test_New_grab3.m
@@ -0,0 +1,189 @@
+%% 外力投影
+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/2000;
+dq=zeros(8,1); 
+% Mphi = diag(5*ones(1,1));
+M = diag(5*ones(1,1));
+% Kphi = diag(40*ones(1,1));
+K = diag(1*ones(1,1));
+C = diag(0*ones(1,1));
+phi_log =[];dq_log =[];ddq_log =[];
+
+Ft(1) = 0;
+
+for i = 1:1000
+    ddq_max = inv(M)*(Ft(i)-K*dq_max-C.*dq_max.^2);
+%     ddq_max = min(1, max(-1, ddq_max)); % dq_lim
+    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));
+    dq_tangent = theta_delta;
+    dtraj_space(:,i) =Jaco*dq_tangent(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_base(4:6)' *  dx(:,i)/norm(dx(:,i));
+    %     Ft(i+1) = Fext(4:6)' * dx(:,i)/(dx(:,i)'*dx(:,i))*dx(:,i);
+%     Fext_base = [0;0;0;20;20;20];
+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_colForce.m b/test_colForce.m
new file mode 100644
index 0000000..552c40d
--- /dev/null
+++ b/test_colForce.m
@@ -0,0 +1,22 @@
+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);
+R1000_Dynamics_num_traj;
\ No newline at end of file
diff --git a/test_grab2.m b/test_grab2.m
index 94875b4..3a11db4 100644
--- a/test_grab2.m
+++ b/test_grab2.m
@@ -29,7 +29,7 @@ theta_init = [0;1.1;-2.1;0.345;0;3.14;0;0.262];
 theta_delta = theta_init - theta_stow;
 traj_space = [];
 
-Vel_phi=zeros(1,1);Acc_phi=zeros(1,1);dt=1/20;phi=zeros(1,1);
+Vel_phi=zeros(1,1);Acc_phi=zeros(1,1);dt=1/2000;phi=zeros(1,1);
 % Mphi = diag(5*ones(1,1));
 Mphi = diag(100*ones(1,1));
 % Kphi = diag(40*ones(1,1));
@@ -39,7 +39,7 @@ phi_log =[];Velphi_log =[];Accphi_log =[];
 
 Ft(1) = 0;
 
-for i = 1:1000
+for i = 1:2000
     Acc_phi = inv(Mphi)*(Ft(i)-Kphi*Vel_phi-Cphi*Vel_phi.^2);
     Vel_phi = Vel_phi + Acc_phi*dt;
     Vel_phi = min(1, max(-1, Vel_phi));