From 9a41c6688d32847504dbde8e8530b7ea3a16d3ca Mon Sep 17 00:00:00 2001 From: cosmic_power Date: Sat, 28 Sep 2024 21:45:14 +0800 Subject: [PATCH] add screw kinematics --- Identification_main.asv | 72 +++++++++++++++++++++++++++++++++++++++++ Identification_main.m | 50 ++++++++++++++++++++++++++-- R1000_Dynamics_num.asv | 46 +++++++++++++------------- R1000_Dynamics_num.m | 32 +++++++++--------- get_Kinematics.m | 19 +++++++++++ get_robot_R1000.asv | 52 ++++++++++++++--------------- get_robot_R1000.m | 4 ++- untitled2.m | 7 ++-- 8 files changed, 212 insertions(+), 70 deletions(-) create mode 100644 Identification_main.asv diff --git a/Identification_main.asv b/Identification_main.asv new file mode 100644 index 0000000..648cbba --- /dev/null +++ b/Identification_main.asv @@ -0,0 +1,72 @@ +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/Identification_main.m b/Identification_main.m index 32b11d8..02488e6 100644 --- a/Identification_main.m +++ b/Identification_main.m @@ -1,7 +1,7 @@ close all;clc;clear file = []; opt.robot_def = 'direct'; -opt.KM_method = 'MDH'; +opt.KM_method = 'SCREW'; opt.Vel_method = 'Direct'; opt.LD_method = 'Direct'; opt.debug = true; @@ -10,7 +10,7 @@ opt.robotName = 'R1000'; opt.Isreal = true; robot = get_robot_R1000(file,opt); % robot.theta = [1,1,0]; -robot = get_Kinematics(robot, opt); + %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; @@ -30,7 +30,51 @@ 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; -R1000_Dynamics_num; +% 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.R(:,:,1) = [[1;0;0],[0;1;0],[0;0;1]]; +robot.Home.R(:,:,2) = [[1;0;0],[0;0;1],[0;-1;0]]; +robot.Home.R(:,:,3) = [[1;0;0],[0;0;1],[0;-1;0]]; +robot.Home.R(:,:,4) = [[1;0;0],[0;0;1],[0;-1;0]]; +robot.Home.R(:,:,5) = [[0;-1;0],[1;0;0],[0;0;1]]; +robot.Home.R(:,:,6) = [[0;-1;0],[0;0;-1],[1;0;0]]; +robot.Home.R(:,:,7) = [[1;0;0],[0;-1;0],[0;0;-1]]; +robot.Home.R(:,:,8) = [[0;0;-1],[1;0;0],[0;-1;0]]; +robot.Home.R(:,:,9) = [[0;0;-1],[0;1;0],[1;0;0]]; +for i=1:9 +robot.Home.P(:,i) = co(:,i); +robot.Home.M(:,:,i) = RpToTrans(robot.Home.R(:,:,i),robot.Home.P(:,i)); +end +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); diff --git a/R1000_Dynamics_num.asv b/R1000_Dynamics_num.asv index 750bd4c..b68e83c 100644 --- a/R1000_Dynamics_num.asv +++ b/R1000_Dynamics_num.asv @@ -24,37 +24,37 @@ for i = 1:N 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=[]; +% Mlist_CG = []; % for i = 0:N-1 % if i == 0 -% M=robot.T(:,:,i+1)*transl(com_pos(:,i+1)); +% 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)); +% 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)*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); + % Mlist_CG=[]; % for i = 1:N % if i == 1 @@ -101,6 +101,8 @@ for i = 1:length(q_J) 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 diff --git a/R1000_Dynamics_num.m b/R1000_Dynamics_num.m index 743d1d0..c7c2ec8 100644 --- a/R1000_Dynamics_num.m +++ b/R1000_Dynamics_num.m @@ -24,17 +24,17 @@ for i = 1:N 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); +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 @@ -44,11 +44,11 @@ for i = 0:N-1 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)); + 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)*RpToTrans(rotation_j,rotation_j*com_pos(:,i+1)); end Mlist_CG = cat(3, Mlist_CG, M); end diff --git a/get_Kinematics.m b/get_Kinematics.m index c9e377d..232e1dc 100644 --- a/get_Kinematics.m +++ b/get_Kinematics.m @@ -1,6 +1,25 @@ function robot = get_Kinematics(robot, opt) if(opt.Isreal) switch opt.KM_method + case 'SCREW' + thetalist = robot.theta; + Slist = robot.slist; + % TODO:move to lib + for j = 1:length(thetalist) + T=robot.Home.M(:,:,j); + for i = j: -1: 1 + T = MatrixExp6(VecTose3(Slist(:, i) * thetalist(i))) * T; + end + robot.TW(:,:,j) = T; + end + for i = 1:size(thetalist) + if i == 1 + robot.T(:,:,i) = robot.TW(:,:,i); + else + robot.T(:,:,i) = TransInv(robot.TW(:,:,i-1))*robot.TW(:,:,i); + end + end + robot.Fkine = robot.TW(:,:,end); case 'SDH' theta = robot.theta; alpha = robot.alpha; diff --git a/get_robot_R1000.asv b/get_robot_R1000.asv index e5cfda2..aaec637 100644 --- a/get_robot_R1000.asv +++ b/get_robot_R1000.asv @@ -73,33 +73,33 @@ switch opt.robot_def % com_pos(:,8) = [0;0.06;0]; % com_pos(:,9) = [0;0;0]; - % com_pos(:,1) = [9.7435250e+01 8.3517655e-01 1.2246547e+02]'*10^-3; - % com_pos(:,2) = [3.7345395e+02 4.4313141e-02 -5.5328829e+01]'*10^-3; - % com_pos(:,3) = [1.8811711e+02 4.9225523e-04 -7.9651429e+00]'*10^-3; - % com_pos(:,4) = [5.4048685e+01 5.8463901e+01 -5.0915631e+00]'*10^-3; - % com_pos(:,5) = [1.3028528e+02 4.8953539e-02 4.6198421e+01]'*10^-3; - % com_pos(:,6) = [4.9059639e+01 5.9928547e-02 -2.8858572e+01]'*10^-3; - % com_pos(:,7) = [2.3210318e-02 -9.5031144e+00 -1.0242124e+02]'*10^-3; - % com_pos(:,8) = [-3.6571935e+01 -3.6282658e-01 -4.7124267e+01]'*10^-3; - % com_pos(:,9) = [-9.6776846e-02 1.4179201e-01 -3.4242667e+01]'*10^-3; + com_pos(:,1) = [9.7435250e+01 8.3517655e-01 1.2246547e+02]'*10^-3; + com_pos(:,2) = [3.7345395e+02 4.4313141e-02 -5.5328829e+01]'*10^-3; + com_pos(:,3) = [1.8811711e+02 4.9225523e-04 -7.9651429e+00]'*10^-3; + com_pos(:,4) = [5.4048685e+01 5.8463901e+01 -5.0915631e+00]'*10^-3; + com_pos(:,5) = [1.3028528e+02 4.8953539e-02 4.6198421e+01]'*10^-3; + com_pos(:,6) = [4.9059639e+01 5.9928547e-02 -2.8858572e+01]'*10^-3; + com_pos(:,7) = [2.3210318e-02 -9.5031144e+00 -1.0242124e+02]'*10^-3; + com_pos(:,8) = [-3.6571935e+01 -3.6282658e-01 -4.7124267e+01]'*10^-3; + com_pos(:,9) = [-9.6776846e-02 1.4179201e-01 -3.4242667e+01]'*10^-3; - com_pos_R1(:,1)=[9.7435250e+01 8.3517655e-01 1.2246547e+02]'; - com_pos_R2(:,1)=[-1.0040117e+02 -9.4164823e+01 -3.4522260e+01]'; - com_pos_R1(:,2)=[3.7345395e+02 -5.5328829e+01 -4.4313141e-02]'; - com_pos_R2(:,2)=[ -1.2654643e+02 9.6371171e+01 -4.4094190e-02]'; - com_pos_R1(:,3)=[1.8811711e+02 -7.9651429e+00 -4.9225523e-04 ]'; - com_pos_R2(:,3)=[-2.6188289e+02 2.8348571e+00 -4.9225523e-04 ]'; - com_pos_R1(:,4)=[ 5.4048685e+01 5.8463901e+01 -5.0915631e+00 ]'; - com_pos_R2(:,4)=[ -6.5951315e+01 -9.0360991e+00 5.2908437e+01]'; - com_pos_R1(:,5)=[1.3028528e+02 4.8953539e-02 4.6198421e+01]'; - com_pos_R2(:,5)=[-9.5814715e+01 4.8953539e-02 -1.2301579e+01 ]'; - com_pos_R1(:,6)=[4.9059639e+01 5.9928547e-02 -2.8858572e+01]'; - com_pos_R2(:,6)=[-4.7403608e+00 5.9928547e-02 6.2741428e+01]'; - com_pos_R1(:,7)=[2.3210318e-02 -9.5031144e+00 -1.0242124e+02]'; - com_pos_R2(:,7)=[2.3210318e-02 -9.5031144e+00 2.0257876e+02 ]'; - com_pos_R1(:,8)=[-3.6571935e+01 -3.6282658e-01 -4.7124267e+01]'; - com_pos_R2(:,8)=[2.2355855e+02 -3.6281380e-01 1.4875409e+01]'; - com_pos_R1(:,9)=[-9.6776846e-02 1.4179201e-01 -3.4242667e+01]'; + 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)=[0 0 0]'*10^-3; % the inertia tensor wrt the frame oriented as the body frame and with the % origin in the COM diff --git a/get_robot_R1000.m b/get_robot_R1000.m index 7afd076..b085191 100644 --- a/get_robot_R1000.m +++ b/get_robot_R1000.m @@ -13,6 +13,8 @@ switch opt.robot_def robot.d = [0,0,0,0,0,0.28,0.40,0,0]; 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']; + case 'SCREW' + robot.theta = zeros(1,9); otherwise disp('Bad opt.KM_method!') return; @@ -51,7 +53,7 @@ switch opt.robot_def robot.accd(ndof)=q_sym(ndof); end % Dynamics parameters - link_mass = [17.42,7.7,2.42,5.19,2.22,1.8,2.31,1.73+1.2,0]; + link_mass = [17.42,7.7,2.42,5.19,2.22,1.8,2.31,1.73,1.2]; %TODO in process, seems axis_of_rot useless axis_of_rot(:,1) = [0;0;1]; axis_of_rot(:,2) = [0;-1;0]; diff --git a/untitled2.m b/untitled2.m index a228bba..a1f8c23 100644 --- a/untitled2.m +++ b/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 COM +% plot 3D: Get 3D coordinate of CO co=[] for i = 1:8 if i == 1 @@ -72,4 +72,7 @@ co = [zeros(3,1),co]; hold on plot3(co(1,:),co(2,:),co(3,:),'o','Color','b') axis equal -grid on \ No newline at end of file +grid on +for i = 1:9 +co(:,i+1)-co(:,i) +end \ No newline at end of file