From f1e0c4951a85d2120f02fcd4d7a27dfead2df909 Mon Sep 17 00:00:00 2001 From: cosmic_power Date: Sun, 20 Apr 2025 22:30:53 +0800 Subject: [PATCH] add grab --- .../calculateGravityForce.m | 19 +++ .../getGravityForce.m | 29 ++++ .../R1000 DVT GravityModel V1/getSimpackF.m | 18 +++ grab_main.asv | 52 -------- test_grab2.asv | 49 ------- test_grab2.m | 124 ++++++++++++++---- 6 files changed, 161 insertions(+), 130 deletions(-) create mode 100644 complie/R1000 DVT GravityModel V1/calculateGravityForce.m create mode 100644 complie/R1000 DVT GravityModel V1/getGravityForce.m create mode 100644 complie/R1000 DVT GravityModel V1/getSimpackF.m delete mode 100644 grab_main.asv delete mode 100644 test_grab2.asv diff --git a/complie/R1000 DVT GravityModel V1/calculateGravityForce.m b/complie/R1000 DVT GravityModel V1/calculateGravityForce.m new file mode 100644 index 0000000..b3bedb3 --- /dev/null +++ b/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); diff --git a/complie/R1000 DVT GravityModel V1/getGravityForce.m b/complie/R1000 DVT GravityModel V1/getGravityForce.m new file mode 100644 index 0000000..24d9f3a --- /dev/null +++ b/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 diff --git a/complie/R1000 DVT GravityModel V1/getSimpackF.m b/complie/R1000 DVT GravityModel V1/getSimpackF.m new file mode 100644 index 0000000..5362d65 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/grab_main.asv b/grab_main.asv deleted file mode 100644 index 5fce8e4..0000000 --- a/grab_main.asv +++ /dev/null @@ -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 \ No newline at end of file diff --git a/test_grab2.asv b/test_grab2.asv deleted file mode 100644 index 7022afe..0000000 --- a/test_grab2.asv +++ /dev/null @@ -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 - diff --git a/test_grab2.m b/test_grab2.m index c775afb..bd4d229 100644 --- a/test_grab2.m +++ b/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_init = theta(1:8); +% theta_stow = [pi/3;1.1;-2.1;0.345;0;3.14;0;0.262]; +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); -Mphi = diag(5*ones(1,1)); -Kphi = diag(40*ones(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(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); - Jaco = JacobianSpace(robot.slist(:,1:8),theta(1:8)); - dtraj_space(:,i) =Jaco*dtheta_phi; %fix + 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 = [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; -view_angle2 = 32; -if isVideoRecordEnable - myVideo = VideoWriter('DVT1_sim'); %open video file - 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]); +% 预先计算所有轨迹点 +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 -for i = 1:1000 +% 图形初始化 +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 - % drawing - DVT.plot([theta_phi(:,i);0]')%机械臂图 - axis([-1 2.0 -1 1 -2 1.6]); - hold on; - view([view_angle1 view_angle2]); - drawnow; - frame = getframe(gcf); %get frame + frame = getframe(gcf); writeVideo(myVideo, frame); end + + drawnow limitrate; % 更高效的刷新方式 +end + +% 确保关闭视频文件 +if isVideoRecordEnable + close(myVideo); + disp('视频录制完成'); end \ No newline at end of file