From b95e3f86b7059a07c7c2a4539c20957655d0217a Mon Sep 17 00:00:00 2001 From: cosmic_power Date: Sun, 29 Sep 2024 02:54:50 +0800 Subject: [PATCH] match with simpack --- Identification_main.asv | 72 ------------------------------------ R1000_Dynamics_num.m | 6 +-- get_robot_R1000.m | 2 - untitled2.asv | 81 ----------------------------------------- untitled2.m | 5 +++ 5 files changed, 8 insertions(+), 158 deletions(-) delete mode 100644 Identification_main.asv delete mode 100644 untitled2.asv diff --git a/Identification_main.asv b/Identification_main.asv deleted file mode 100644 index 648cbba..0000000 --- a/Identification_main.asv +++ /dev/null @@ -1,72 +0,0 @@ -close all;clc;clear -file = []; -opt.robot_def = 'direct'; -opt.KM_method = 'MDH'; -opt.Vel_method = 'Direct'; -opt.LD_method = 'Direct'; -opt.debug = true; -opt.robotName = 'R1000'; - -opt.Isreal = true; -robot = get_robot_R1000(file,opt); -% robot.theta = [1,1,0]; - -%TODO verify kinematics via robotics toolbox or other software result -com_pos_R1(:,1)=[9.7435250e+01 8.3517655e-01 1.2246547e+02]'*10^-3; -com_pos_R2(:,1)=[-1.0040117e+02 -9.4164823e+01 -3.4522260e+01]'*10^-3; -com_pos_R1(:,2)=[3.7345395e+02 -5.5328829e+01 -4.4313141e-02]'*10^-3; -com_pos_R2(:,2)=[ -1.2654643e+02 9.6371171e+01 -4.4094190e-02]'*10^-3; -com_pos_R1(:,3)=[1.8811711e+02 -7.9651429e+00 -4.9225523e-04 ]'*10^-3; -com_pos_R2(:,3)=[-2.6188289e+02 2.8348571e+00 -4.9225523e-04 ]'*10^-3; -com_pos_R1(:,4)=[ 5.4048685e+01 5.8463901e+01 -5.0915631e+00 ]'*10^-3; -com_pos_R2(:,4)=[ -6.5951315e+01 -9.0360991e+00 5.2908437e+01]'*10^-3; -com_pos_R1(:,5)=[1.3028528e+02 4.8953539e-02 4.6198421e+01]'*10^-3; -com_pos_R2(:,5)=[-9.5814715e+01 4.8953539e-02 -1.2301579e+01 ]'*10^-3; -com_pos_R1(:,6)=[4.9059639e+01 5.9928547e-02 -2.8858572e+01]'*10^-3; -com_pos_R2(:,6)=[-4.7403608e+00 5.9928547e-02 6.2741428e+01]'*10^-3; -com_pos_R1(:,7)=[2.3210318e-02 -9.5031144e+00 -1.0242124e+02]'*10^-3; -com_pos_R2(:,7)=[2.3210318e-02 -9.5031144e+00 2.0257876e+02 ]'*10^-3; -com_pos_R1(:,8)=[-3.6571935e+01 -3.6282658e-01 -4.7124267e+01]'*10^-3; -com_pos_R2(:,8)=[2.2355855e+02 -3.6281380e-01 1.4875409e+01]'*10^-3; % don't use -com_pos_R1(:,9)=[-9.6776846e-02 1.4179201e-01 -3.4242667e+01]'*10^-3; - -% Get 3D coordinate of CO -co=[] -for i = 1:8 - if i == 1 - co(:,i) = com_pos_R1(:,i)-com_pos_R2(:,i); - elseif i<8 - %From base to ISA Origin - co(:,i) = co(:,i-1)+com_pos_R1(:,i)-com_pos_R2(:,i); - else - %From base to ISA Origin - co(:,i) = co(:,i-1)-[0;0;0.05896]; - end -end -co = [zeros(3,1),co]; - -% temp slist -robot.slist=[[0;0;1;0;0;0],... - [0;-1;0;cross(-[0;-1;0],[0.2;0;0])]... - [0;-1;0;cross(-[0;-1;0],[0.2+0.5;0;0])]... - [0;-1;0;cross(-[0;-1;0],[0.2+0.5+0.45;0;0])]... - [0;0;1;cross(-[0;0;1],[0.2+0.5+0.45+0.12;0;0])]... - [1;0;0;cross(-[1;0;0],[0.2+0.5+0.45+0.12+0.28;0;0])]... - [0;0;-1;cross(-[0;0;-1],[0.2+0.5+0.45+0.12+0.28;0;-0.4])]... - [0;-1;0;cross(-[0;-1;0],[0.2+0.5+0.45+0.12+0.28;0;-0.4])]... - [0;0;0;1;0;0]]; - -robot.slist(4:6,:) = co; -robot.Home = [0;0;-1]; -robot.alpha = [0,pi/2,0,0,-pi/2,-pi/2,-pi/2,-pi/2,-pi/2]; -robot.link_type = ['R','R','R','R','R','R','R','R','P']; - -robot = get_Kinematics(robot, opt); - -% R1000_Dynamics_num; -% opt.Isreal = false; -% robot = get_velocity(robot, opt); -% robot = get_regressor(robot,opt); -% symbol matched -% verify_regressor -% robot = get_baseParams(robot, opt); \ No newline at end of file diff --git a/R1000_Dynamics_num.m b/R1000_Dynamics_num.m index a3bdfca..6740780 100644 --- a/R1000_Dynamics_num.m +++ b/R1000_Dynamics_num.m @@ -12,9 +12,9 @@ link_mass = robot.m; com_pos = robot.com; link_inertia = robot.I; -thetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]'; -dthetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]'; -ddthetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]'; +thetalist = [q_J;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]'; +dthetalist = [qd_J;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]'; +ddthetalist = [qdd_J;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]'; % Get general mass matrix Glist=[]; diff --git a/get_robot_R1000.m b/get_robot_R1000.m index 673cbe9..b085191 100644 --- a/get_robot_R1000.m +++ b/get_robot_R1000.m @@ -15,8 +15,6 @@ switch opt.robot_def robot.link_type = ['R','R','R','R','R','R','R','R','P']; case 'SCREW' robot.theta = zeros(1,9); - robot.theta(5) = pi/3; - robot.theta(7) = pi/3; otherwise disp('Bad opt.KM_method!') return; diff --git a/untitled2.asv b/untitled2.asv deleted file mode 100644 index e755b92..0000000 --- a/untitled2.asv +++ /dev/null @@ -1,81 +0,0 @@ -Mlist_CG=[] -for i = 0:N-1 - if i == 0 - M=robot.T(:,:,i+1)*transl(com_pos(:,i+1)); - else - % rotation_i = TransToRp(robot.T(:,:,i+1)) - M = TransInv(transl(com_pos(:,i)))*robot.T(:,:,i+1)*transl(com_pos(:,i+1)); - end - Mlist_CG = cat(3, Mlist_CG, M); -end -%% -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,com_pos(:,i)))*robot.T(:,:,i+1)*RpToTrans(rotation_j,com_pos(:,i+1)); - end - Mlist_CG = cat(3, Mlist_CG, M); -end -%% -com_pos_R1(:,1)=[9.7435250e+01 8.3517655e-01 1.2246547e+02]'*10^-3; -com_pos_R2(:,1)=[-1.0040117e+02 -9.4164823e+01 -3.4522260e+01]'*10^-3; -com_pos_R1(:,2)=[3.7345395e+02 -5.5328829e+01 -4.4313141e-02]'*10^-3; -com_pos_R2(:,2)=[ -1.2654643e+02 9.6371171e+01 -4.4094190e-02]'*10^-3; -com_pos_R1(:,3)=[1.8811711e+02 -7.9651429e+00 -4.9225523e-04 ]'*10^-3; -com_pos_R2(:,3)=[-2.6188289e+02 2.8348571e+00 -4.9225523e-04 ]'*10^-3; -com_pos_R1(:,4)=[ 5.4048685e+01 5.8463901e+01 -5.0915631e+00 ]'*10^-3; -com_pos_R2(:,4)=[ -6.5951315e+01 -9.0360991e+00 5.2908437e+01]'*10^-3; -com_pos_R1(:,5)=[1.3028528e+02 4.8953539e-02 4.6198421e+01]'*10^-3; -com_pos_R2(:,5)=[-9.5814715e+01 4.8953539e-02 -1.2301579e+01 ]'*10^-3; -com_pos_R1(:,6)=[4.9059639e+01 5.9928547e-02 -2.8858572e+01]'*10^-3; -com_pos_R2(:,6)=[-4.7403608e+00 5.9928547e-02 6.2741428e+01]'*10^-3; -com_pos_R1(:,7)=[2.3210318e-02 -9.5031144e+00 -1.0242124e+02]'*10^-3; -com_pos_R2(:,7)=[2.3210318e-02 -9.5031144e+00 2.0257876e+02 ]'*10^-3; -com_pos_R1(:,8)=[-3.6571935e+01 -3.6282658e-01 -4.7124267e+01]'*10^-3; -com_pos_R2(:,8)=[2.2355855e+02 -3.6281380e-01 1.4875409e+01]'*10^-3; % don't use this -com_pos_R1(:,9)=[-9.6776846e-02 1.4179201e-01 -3.4242667e+01]'*10^-3; -% plot 3D: Get 3D coordinate of COM -ct=[]; -for i = 1:9 - 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;0;0.05896]+com_pos_R1(:,i); - end -end -plot3(ct(1,:),ct(2,:),ct(3,:),'o','Color','r') -axis equal -grid on -% plot 3D: Get 3D coordinate of CO -co=[]; -for i = 1:8 - if i == 1 - co(:,i) = com_pos_R1(:,i)-com_pos_R2(:,i); - elseif i<8 - %From base to ISA Origin - co(:,i) = co(:,i-1)+com_pos_R1(:,i)-com_pos_R2(:,i); - else - %From base to ISA Origin - co(:,i) = co(:,i-1)-[0;0;0.05896]; - end -end -co = [zeros(3,1),co]; -hold on -plot3(co(1,:),co(2,:),co(3,:),'o','Color','b') -axis equal -grid on -%% -for i = 1:9 -co(:,i+1)-co(:,i); -end -%% -robot. \ No newline at end of file diff --git a/untitled2.m b/untitled2.m index a9fbcb9..cef6b9d 100644 --- a/untitled2.m +++ b/untitled2.m @@ -81,6 +81,11 @@ end for i = 1:9 tt(:,i)=robot.TW(1:3,1:3,i)*com_pos_R1(:,i); end +kk=[]; +for i = 1:9 +kk(:,:,i)=TransInv(robot.TW(:,:,i))*Mlist_CG_Base(:,:,i); +end + %% yy=eye(4,4); for i =1:9