add grab

complie/R1000 DVT GravityModel V1/calculateGravityForce.m

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

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

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

grab_main.asv

@@ -1,52 +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];
-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);
-% Ft = Fext*dtraj_space; %fix
-% % saturation and switch
-% %%
-Vel_phi=zeros(6,1);Acc_phi=zeros(6,1);dt=1/2000;phi=zeros(6,1);
-Mphi = diag(10);
-Kphi = diag(ones(1,6));
-Cphi = diag(ones(1,6));
-phi_log =[];
-for i = 1:100
-Acc_phi = inv(Mphi)*(Fext(i)-Kphi*Vel_phi-Cphi*Vel_phi.^2);
-Vel_phi = Vel_phi + Acc_phi*dt;
-phi = phi + Vel_phi*dt;
-phi_log = [phi_log,phi];
-end

test_grab2.asv

@@ -1,49 +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 = [pi;0;0;0;0;0;0;0];
-theta_delta = theta(1:8) - theta_stow;
-theta 
-traj_space = [];
-for i = 1:1000
-    phi = linspace(0,1,100).*ones(8,1); %?
-    theta_phi = theta(1:8) - theta_delta .* phi;
-    dtheta_phi = -theta_delta .* linspace(1,1,100).*ones(8,1);
-    
-    %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(:,i); %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

@@ -20,22 +20,26 @@ get_Kinematics_func = sprintf('get_Kinematics_%s',opt.robotName);
 robot = feval(get_Kinematics_func,robot,opt);
 
 theta_stow = deg2rad([0;113;-160;47;80;90;90;0]);
-% theta_stow = [2*pi;0;0;0;0;0;0;0];
+% theta_stow = [pi/3;1.1;-2.1;0.345;0;3.14;0;0.262];
-theta_init = theta(1:8);
+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/10;phi=zeros(1,1);
+Vel_phi=zeros(1,1);Acc_phi=zeros(1,1);dt=1/20;phi=zeros(1,1);
-Mphi = diag(5*ones(1,1));
+% Mphi = diag(5*ones(1,1));
-Kphi = diag(40*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 =[];
+phi_log =[];Velphi_log =[];Accphi_log =[];
 
 Ft(1) = 0;
 
 for i = 1:1000
     Acc_phi = inv(Mphi)*(Ft(i)-Kphi*Vel_phi-Cphi*Vel_phi.^2);
+    Acc_phi = min(0.1, max(-0.1, Acc_phi));
     Vel_phi = Vel_phi + Acc_phi*dt;
+    Vel_phi = min(0.01, max(-0.01, Vel_phi));
     phi = phi + Vel_phi*dt;
     if phi>=1
         phi=1;
@@ -46,6 +50,8 @@ for i = 1:1000
         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;
@@ -58,17 +64,43 @@ for i = 1:1000
     get_Kinematics_func = sprintf('get_Kinematics_%s',opt.robotName);
     robot = feval(get_Kinematics_func,robot,opt);
     %FK
-    traj_space(:,:,i) = robot.kine.TW(:,:,8);
+    traj_space(:,:,i) = robot.kine.TW(:,:,7);
-    Jaco = JacobianSpace(robot.slist(:,1:8),theta(1:8));
+    Jaco = JacobianSpace(robot.slist(:,1:7),theta(1:7));
-    dtraj_space(:,i) =Jaco*dtheta_phi; %fix
+    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 = [0;0;0;10;10;0];
+    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(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) = 1;
+%     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');
@@ -81,27 +113,61 @@ L7=Link('revolute', 'd', 0.40, 'a', 0, 'alpha', -pi/2, 'offset',-pi/2,'qlim',deg
 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])%机械臂图
-isVideoRecordEnable = 1;
+% 预先计算所有轨迹点
-view_angle1 = -136;
+traj_points = zeros(3, 100);  % 假设总共有100个点
-view_angle2 = 32;
+for i = 1:100
-if isVideoRecordEnable 
+    T_L8 = DVT_L8.fkine(theta_phi(:,i)); 
-    myVideo = VideoWriter('DVT1_sim'); %open video file
+    traj_points(:,i) = T_L8.t;  % 存储所有轨迹点
-    myVideo.FrameRate = 30;  %can adjust this, 5 - 10 works well for me
-    open(myVideo);
-    axis([-1 2.0 -1 1 -2 1.6]);
-    view([view_angle1 view_angle2]);
 end
 
-for i = 1:1000
+% 图形初始化
-    if isVideoRecordEnable
+figure;
-        % drawing
+axis([-1 2.0 -1 2 -2 1.6]);
-        DVT.plot([theta_phi(:,i);0]')%机械臂图
+view_angle1 = -156;view_angle2=46;
-        axis([-1 2.0 -1 1 -2 1.6]);
-        hold on;
 view([view_angle1 view_angle2]);
-        drawnow;
+isVideoRecordEnable = 0;
-        frame = getframe(gcf); %get frame
+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