From 91faa14682a4ce8393c03a1daf6f4ddee327b8f2 Mon Sep 17 00:00:00 2001 From: cosmic_power Date: Mon, 23 Sep 2024 23:28:41 +0800 Subject: [PATCH] add real datas --- R1000_Dynamics_num.m | 15 ++++--- get_robot_R1000.m | 94 ++++++++++++++++++++++++++++++++------------ 2 files changed, 77 insertions(+), 32 deletions(-) diff --git a/R1000_Dynamics_num.m b/R1000_Dynamics_num.m index 87eb2d6..fa5b892 100644 --- a/R1000_Dynamics_num.m +++ b/R1000_Dynamics_num.m @@ -1,8 +1,8 @@ %% R1000 N=9; % traj -time = 0:0.01:2; -f=1; +time = 0:0.01:1; +f=1; q_J = sin(2*pi*f*time); qd_J = (2*pi*f)*cos(2*pi*f*time); qdd_J = -(2*pi*f)^2*sin(2*pi*f*time); @@ -12,9 +12,9 @@ link_mass = robot.m; com_pos = robot.com; link_inertia = robot.I; -thetalist = [q_J;zero_;zero_;zero_;zero_;q_J;zero_;zero_;zero_]'; -dthetalist = [qd_J;zero_;zero_;zero_;zero_;qd_J;zero_;zero_;zero_]'; -ddthetalist = [qdd_J;zero_;zero_;zero_;zero_;qdd_J;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=[]; @@ -71,7 +71,10 @@ for i = 1:length(q_J) F_Simpack(:,:,i) = getSimpackF(G(:,:,:,i),T(:,:,:,i),Mlist_ED,Fmat(:,:,i)); end % plot Torque -F_Simpack = pagetranspose(F_Simpack); +% above 2020b +% F_Simpack = pagetranspose(F_Simpack); +% below 2020b +F_Simpack = permute(F_Simpack,[2 1 3]); figure(1) for i = 1:3 subplot(3,1,i); diff --git a/get_robot_R1000.m b/get_robot_R1000.m index 9c8a038..bc9d741 100644 --- a/get_robot_R1000.m +++ b/get_robot_R1000.m @@ -12,18 +12,17 @@ switch opt.robot_def robot.a = [0,0.2,0.5,0.45,0.12,0,0,0, 0.126493]; robot.d = [0,0,0,0,0,0.28,0.40,0,-0.3157]; 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']; otherwise disp('Bad opt.KM_method!') return; end else % Create symbolic generilized coordiates, their first and second deriatives - q_sym = sym('q%d',[ndof+1,1],'real'); - qd_sym = sym('qd%d',[ndof+1,1],'real'); - q2d_sym = sym('qdd%d',[ndof+1,1],'real'); - q_sym(ndof+1) = 0; - qd_sym(ndof+1) = 0; - q2d_sym(ndof+1) = 0; + q_sym = sym('q%d',[ndof,1],'real'); + qd_sym = sym('qd%d',[ndof,1],'real'); + q2d_sym = sym('qdd%d',[ndof,1],'real'); + robot.theta = q_sym; robot.dtheta = qd_sym; robot.ddtheta = q2d_sym; @@ -35,20 +34,21 @@ switch opt.robot_def case 'SDH' case 'MDH' robot.a = [0,0.2,0.5,0.45,0.12,0,0,0, 0.126493]; - robot.d = [0,0,0,0,0,0.28,0.40,0,0]; + robot.d = [0,0,0,0,0,0.28,0.40,0,-0.3157]; 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']; otherwise disp('Bad opt.KM_method!') return; end - robot.d(ndof)=q_sym; + robot.d = sym(robot.d); + robot.d(ndof)=q_sym(ndof); %init vd robot.vd = robot.d; - robot.vd(ndof)=q_sym; + robot.vd(ndof)=q_sym(ndof); %init accd robot.accd = robot.d; - robot.accd(ndof)=q_sym; + robot.accd(ndof)=q_sym(ndof); end % Dynamics parameters link_mass = [17.42,7.7,2.42,5.16,2.22,1.78,2.32,2.92 0.1]; @@ -63,26 +63,68 @@ switch opt.robot_def axis_of_rot(:,8) = [0;0;1]; axis_of_rot(:,9) = [0;0;1]; % 画图 - com_pos(:,1) = [0;0;0.122]; - com_pos(:,2) = [0.373;0;0]; - com_pos(:,3) = [0.188;0;0]; - com_pos(:,4) = [0.05;0;-0.05]; - com_pos(:,5) = [0;0.13;0]; - com_pos(:,6) = [0;0.028;0.049]; - com_pos(:,7) = [0;0;0.102]; - com_pos(:,8) = [0;0.06;0]; +% com_pos(:,1) = [0;0;0.122]; +% com_pos(:,2) = [0.373;0;0]; +% com_pos(:,3) = [0.188;0;0]; +% com_pos(:,4) = [0.05;0;-0.05]; +% com_pos(:,5) = [0;0.13;0]; +% com_pos(:,6) = [0;0.028;0.049]; +% com_pos(:,7) = [0;0;0.102]; +% 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.3924501e+01 -5.1146307e+00 -5.8247754e+01]'*10^-3; + com_pos(:,5) = [1.3028528e+02 4.8953539e-02 4.6198421e+01]'*10^-3; + com_pos(:,6) = [-6.0339586e-02 2.8867234e+01 4.9027126e+01]'*10^-3; + com_pos(:,7) = [ -2.3140597e-02 -9.5969566e+00 1.0229381e+02]'*10^-3; + com_pos(:,8) = [1.4847544e+01 6.8637348e+01 -1.5713395e-01]'*10^-3; com_pos(:,9) = [0;0;0]; % the inertia tensor wrt the frame oriented as the body frame and with the % origin in the COM - link_inertia(:,:,1) = diag([1,1,1]); - link_inertia(:,:,2) = diag([1,1,1]); - link_inertia(:,:,3) = diag([1,1,1]); - link_inertia(:,:,4) = diag([1,1,1]); - link_inertia(:,:,5) = diag([1,1,1]); - link_inertia(:,:,6) = diag([1,1,1]); - link_inertia(:,:,7) = diag([1,1,1]); - link_inertia(:,:,8) = diag([1,1,1]); +% link_inertia(:,:,1) = diag([1,1,1]); +% link_inertia(:,:,2) = diag([1,1,1]); +% link_inertia(:,:,3) = diag([1,1,1]); +% link_inertia(:,:,4) = diag([1,1,1]); +% link_inertia(:,:,5) = diag([1,1,1]); +% link_inertia(:,:,6) = diag([1,1,1]); +% link_inertia(:,:,7) = diag([1,1,1]); +% link_inertia(:,:,8) = diag([1,1,1]); +% link_inertia(:,:,9) = diag([1,1,1]); + link_inertia(:,:,1) = [[1.1212091e+05 -1.1694021e+04 -4.7186593e+04];... + [-1.1694021e+04 2.3289532e+05 -3.0395414e+02];... + [-4.7186593e+04 -3.0395414e+02 2.0033875e+05]]*10^-6; + link_inertia(:,:,2) = [[3.7926328e+04 -9.0569033e+01 4.7526575e+04];... + [-9.0569033e+01 2.9714754e+05 6.8396715e+00];... + [4.7526575e+04 6.8396715e+00 2.8138392e+05]]*10^-6; + link_inertia(:,:,3) = [[4.4513887e+03 1.9981964e-01 -3.0303891e+02];... + [1.9981964e-01 6.7952039e+04 -8.8585864e-02];... + [-3.0303891e+02 -8.8585864e-02 6.9958344e+04]]*10^-6; + link_inertia(:,:,4) = [[1.1642351e+04 2.2997175e+03 2.9159431e+03];... + [2.2997175e+03 2.6031269e+04 -1.3518384e+02];... + [2.9159431e+03 -1.3518384e+02 2.4694742e+04]]*10^-6; + link_inertia(:,:,5) = [[3.0930544e+03 8.3558814e-01 -2.8169092e+03];... + [8.3558814e-01 1.2796446e+04 -3.3666469e+00];... + [-2.8169092e+03 -3.3666469e+00 1.2128856e+04]]*10^-6; + link_inertia(:,:,6) = [[3.6635776e+03 -7.0081461e+00 2.2392870e+00];... + [-7.0081461e+00 1.8152305e+03 -2.4828765e+02];... + [2.2392870e+00 -2.4828765e+02 3.4602935e+03]]*10^-6; + link_inertia(:,:,7) = [[1.3662652e+04 -3.6340953e+00 4.4011670e-01];... + [-3.6340953e+00 1.3222824e+04 -4.3625500e+02];... + [ 4.4011670e-01 -4.3625500e+02 2.2500397e+03]]*10^-6; + link_inertia(:,:,8) = [[4.6491328e+03 3.0225715e+03 2.8800116e+01];... + [3.0225715e+03 8.8414058e+04 -3.0084286e+01];... + [2.8800116e+01 -3.0084286e+01 9.1858852e+04]]*10^-6; link_inertia(:,:,9) = diag([1,1,1]); + % verify if link_inertia is issymmetric + for i = 1:ndof + if(issymmetric(link_inertia(:,:,i))==false) + fprintf('Bad definition of inertia matrix %d\n',i) + return; + end + end % manipulator regressor for i = 1:ndof robot.m(i) = link_mass(i);