Identification_main.m

@@ -30,5 +30,5 @@ robot = get_regressor(robot,opt);
 % % verify_regressor_R1000;
 robot = get_baseParams(robot, opt);
 % robot = estimate_dyn(robot,opt);
-robot = estimate_dyn_form_data(robot,opt);
-% robot = estimate_dyn_MLS(robot,opt);
+% robot = estimate_dyn_form_data(robot,opt);
+robot = estimate_dyn_MLS(robot,opt);

R1000_Dynamics_num_traj.m

@@ -1,259 +0,0 @@
-%% 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);

Untitled4.m

@@ -1,26 +0,0 @@
-% 定义phi的范围
-phi = linspace(0, 10, 100); % 从0到10，100个点
-
-% 初始化向量数组
-x = zeros(size(phi));
-y = zeros(size(phi));
-z = (1 - exp(-3*phi)) ./ (1 - exp(-3));
-
-% 创建图形
-figure;
-hold on;
-axis equal;
-grid on;
-
-% 绘制向量
-plot(phi,z);
-
-% 设置图形属性
-% xlabel('X');
-% ylabel('Y');
-zlabel('Z');
-title('Vector Field Trajectory');
-legend('Trajectory');
-
-% % 设置视角
-% view(3);

complie/R1000 DVT GravityModel V1/calculateGravityForce.m

@@ -1,19 +0,0 @@
-function GravityForce  = calculateGravityForce(thetaMea)
-% Example:
-% thetalist = [0;0;0;0;0;pi/2;0;0;0];
-% calculateGravityForce([thetalist])
-
-% code start
-opt.robot_def = 'direct';
-opt.KM_method = 'SCREW';
-opt.Vel_method = 'Direct';
-opt.LD_method = 'Direct';
-opt.debug = true;
-opt.robotName = 'R1000_DVT';
-opt.reGenerate = false;
-opt.Isreal = true;
-zero_ = zeros(length(thetaMea),1);
-
-robot = get_robot_R1000_DVT(zero_,zero_,zero_,opt);
-robot = get_Kinematics_R1000_DVT(robot, opt);
-GravityForce = getGravityForce(thetaMea, robot, opt);

complie/R1000 DVT GravityModel V1/getGravityForce.m

@@ -1,29 +0,0 @@
-function GravityForce = getGravityForce(thetalist,robot,opt)
-dthetalist = zeros([robot.ndof,1]);
-ddthetalist = zeros([robot.ndof,1]);
-exf = zeros([6,1]);
-
-% Get info from robot
-Mlist_CG = robot.kine.Mlist_CG;
-Mlist_ED = robot.kine.Mlist_ED;
-Slist = robot.slist;
-
-% Get general mass matrix
-Glist=zeros(6,6,robot.ndof);
-link_inertia = zeros(3,3,robot.ndof);
-for i = 1:robot.ndof
-    link_inertia(:,:,i) = robot.Home.R(:,:,i)'*robot.I(:,:,i)*robot.Home.R(:,:,i);
-    Gb= [link_inertia(:,:,i),zeros(3,3);zeros(3,3),robot.m(i)*diag([1,1,1])];
-    Glist(:,:,i) = Gb;
-end
-
-[~,~,~,Fmat,~] ...
-    = InverseDynamics_debug(thetalist, dthetalist, ddthetalist, ...
-    robot.gravity, exf, Mlist_CG, Glist, Slist);
-G = FKinSpaceExpand(Mlist_CG, Slist, thetalist);
-T = FKinSpaceExpand(Mlist_ED, Slist, thetalist);
-F_Simpack = getSimpackF(G,T,Mlist_ED,Fmat);
-% get J7
-permutationMatrix = diag([1,1,-1,1,1,-1]);
-GravityForce = permutationMatrix*F_Simpack(:,7);
-end

complie/R1000 DVT GravityModel V1/getSimpackF.m

@@ -1,18 +0,0 @@
-function Flist = getSimpackF(T_GC,T_O,Mlist,Fmat)
-N=size(Fmat,1);
-Flist = zeros(6,N);
-
-%base frame
-% Adgab = Adjoint(TransInv(T_GC(:,:,1))*Mlist(:,:,1));
-% Flist(:,1) = Adgab'*Fmat(1,:)';
-% for i = 2: N
-% %     Adgab = Adjoint(TransInv(T_GC(:,:,i))*T_O(:,:,i-1)*Mlist(:,:,i));
-%     Adgab = Adjoint(TransInv(T_GC(:,:,i))*T_O(:,:,i)); 
-%     Flist(:,i) = Adgab'*Fmat(i,:)';
-% endAdgab = Adjoint(TransInv(T_GC(:,:,1))*Mlist(:,:,1));
-for i = 1: N
-%     Adgab = Adjoint(TransInv(T_GC(:,:,i))*T_O(:,:,i-1)*Mlist(:,:,i));
-    Adgab = Adjoint(TransInv(T_GC(:,:,i))*T_O(:,:,i)); 
-    Flist(:,i) = Adgab'*Fmat(i,:)';
-end
-end

getTorqueData.m

@@ -1,74 +0,0 @@
-% 加载数据文件
-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

get_robot_R1000_DVT.m

@@ -39,7 +39,7 @@ switch opt.robot_def
         robot.com_pos_R1 = com_pos_R1;
         robot.com_pos_R2 = com_pos_R2;
         % FIXME:fix this hack
-        % Get 3D coordinate of next joint axis
+        % Get 3D coordinate of CO
         co=[];
         for i = 1:8
             if i == 1

grab_main.m

@@ -1,63 +0,0 @@
-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;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;

test_New_grab.m

@@ -1,201 +0,0 @@
-%% 外力投影
-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(0.005*ones(1,1));
-% Kphi = 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: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));
-    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)=-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
-%%
-% 预先创建箭头对象（初始位置和方向可以随意设置，后续会更新）
-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

test_New_grab2.m

@@ -1,188 +0,0 @@
-%% 外力投影
-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

test_New_grab3.m

@@ -1,189 +0,0 @@
-%% 外力投影
-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

test_colForce.m

@@ -1,22 +0,0 @@
-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;

test_grab.m

@@ -1,50 +0,0 @@
-%% 外力投影
-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
-
-

test_grab2.m

@@ -1,176 +0,0 @@
-%% 外力投影
-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;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 = [];
-
-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));
-Kphi = diag(80*ones(1,1));
-Cphi = diag(0*ones(1,1));
-phi_log =[];Velphi_log =[];Accphi_log =[];
-
-Ft(1) = 0;
-
-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));
-    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
-    Accphi_log = [Accphi_log,Acc_phi];
-    Velphi_log = [Velphi_log,Vel_phi];
-    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(:,:,7);
-    Jaco = JacobianSpace(robot.slist(:,1:7),theta(1:7));
-    dtraj_space(:,i) =Jaco*dtheta_phi(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);
-    Ft(i+1) =  Fext_base(4:6)' *  dx(:,i)/norm(dx(:,i));
-%     Ft(i+1) = sin(2*pi*dt*i);
-%     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:1000
-    % 绘制机械臂
-    %     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_phi(:,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 = 0;
-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_phi(:,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

untitled2.m

@@ -55,7 +55,7 @@ end
 plot3(ct(1,:),ct(2,:),ct(3,:),'o','Color','r')
 axis equal
 grid on
-% plot 3D: Get 3D coordinate of next joint axis
+% plot 3D: Get 3D coordinate of CO
 co=[];
 for i = 1:8
     if i == 1