├── InverseDynamic ├── Lagrange_InverseDynamics.m ├── MCG.mat ├── Newtown_InverseDynamics.m ├── Result_L.mat ├── RobotTool_Verify.m ├── readMe.txt └── test.m ├── analysis_process_new_euler ├── M&C&G.mat ├── Traj_10s_11.mat ├── calculteCCandKK.m ├── calculteGandM.m ├── calculteKK.m ├── compute_frame_transform.m ├── insideEquation.m ├── new.m ├── outsideEquation.m └── 解析结果 │ ├── CC&KK.mat │ ├── M&C&G.mat │ └── tau.mat └── readMe.txt /InverseDynamic/Lagrange_InverseDynamics.m: -------------------------------------------------------------------------------- 1 | function [Jee,M,C,G] = Lagrange_InverseDynamics(dh,rob,Pc,Ic,g,m,q,dq) 2 | %@input: dh stanard for modified DH, dh_list = [alpha; a; d; theta]; DH(4X6). 3 | %@input: int rob the 0 standard for rotation, the 1 standard for translation. but also length(rob) equal size(dh,2); rob(1X6). 4 | %@input:Pc sandard the mess center of link, example: Pc(1,1) standsrd for the center of x axis on first link, Pc(2,1)standsrd for the center of y axis on first link,Pc(3,1)standsrd for the center of z axis on first link Pc(3X6). 5 | %@input:Ic sandard the inertia tensor of link,example:Ic(:,:,1)standard for the first link inertia tensor, and Ic(:,:,1) is a symmetry matrix Ic(3X3X6). 6 | %@input:g:9.81. 7 | %@input:m:the mess of link, m(1X6). 8 | %@input:q:joint angle for every link, q(1X6). 9 | %@input:dq:joint angle velocity for every link, dq(1X6). 10 | 11 | %@output:Pee: the end position for robot, respect standard for the position of x y z axis. Pee(3X1). 12 | %@output:Ree: the end rotation for robot, it's is a rotation traslation matrix. Ree(3X3). 13 | %@output:Jee: jacobians for robot contain linear and angle part, Jee(6X6). 14 | %@output:M:M(q), M(6X6). 15 | %@output:C:C(q,dq), C(6X6). 16 | %@output:G:G(q), G(1X6). 17 | %% parameters 18 | alpha = dh(1,:); 19 | a = dh(2,:); 20 | d = dh(3,:); 21 | theta = dh(4,:); 22 | N = size(dh,2); 23 | for i=1:N 24 | pc{i}=Pc(:,i); 25 | end 26 | for i=1:N 27 | if alpha(i)==0 28 | Salpha(i)=0; 29 | Calpha(i)=1; 30 | elseif abs(alpha(i))==pi/2 31 | Calpha(i)=0; 32 | if alpha(i)==pi/2 33 | Salpha(i)=1; 34 | elseif alpha(i)==-pi/2 35 | Salpha(i)=-1; 36 | end 37 | elseif abs(alpha(i))==pi 38 | Salpha(i)=0; 39 | Calpha(i)=-1; 40 | else 41 | Salpha(i)=sin(alpha(i)); 42 | Calpha(i)=cos(alpha(i)); 43 | end 44 | end 45 | 46 | %% R 47 | for i=1:N 48 | if rob(i)==0 % Rotation matrix of revolute joints 49 | R{i}=[cos(q(i)) -sin(q(i)) 0; 50 | sin(q(i))*Calpha(i) cos(q(i))*Calpha(i) -Salpha(i); 51 | sin(q(i))*Salpha(i) cos(q(i))*Salpha(i) Calpha(i)]; 52 | elseif rob(i)==1 % Rotation matrix of prismatic joints 53 | R{i}=[cos(theta(i)) -sin(theta(i)) 0; 54 | sin(theta(i))*Calpha(i) cos(theta(i))*Calpha(i) -Salpha(i); 55 | sin(theta(i))*Salpha(i) cos(theta(i))*Salpha(i) Calpha(i)]; 56 | end 57 | end 58 | 59 | %% P 60 | for i=1:N 61 | if rob(i)==0 % Position vector of revolute joints 62 | p{i}=[a(i);-Salpha(i)*d(i);Calpha(i)*d(i)]; 63 | elseif rob(i)==1 % Position vector of prismatic joints 64 | p{i}=[a(i);-Salpha(i)*q(i);Calpha(i)*q(i)]; 65 | end 66 | end 67 | 68 | 69 | for i=1:N 70 | pic{i}=p{i}+R{i}*pc{i}; 71 | end 72 | 73 | poc{1}=pic{1}; 74 | for i=2:N 75 | nic=pic{i}; 76 | for j=i-1:-1:1 77 | nic=p{j}+R{j}*nic; 78 | end 79 | poc{i}=nic; 80 | end 81 | Pee = poc{N}; 82 | 83 | %% R0{i} stand for 0--> ith framework rotation transition matrix 84 | R0{1}=R{1}; 85 | for i=2:N 86 | R0{i}=simplify(R0{i-1}*R{i}); 87 | end 88 | Ree = R0{N}; 89 | 90 | %% linear part of jacobians 91 | for i=1:N 92 | Jv{i}=simplify(jacobian(poc{i},q)); 93 | end 94 | 95 | %% angular part of jacobians 96 | if rob(1)==0 97 | Jo{1}=simplify([R0{1}(:,3),zeros(3,N-1)]); 98 | elseif rob(1)==1 99 | Jo{1}=zeros(3,N); 100 | end 101 | 102 | for i=2:N 103 | if rob(i)==1 104 | Jo{i}=zeros(3,N); 105 | elseif rob(i)==0 106 | Jo{i}=Jo{i-1}; 107 | Jo{i}(:,i)=R0{i}(:,3); 108 | end 109 | end 110 | Jee = [Jv{N};Jo{N}]; 111 | 112 | %% M 113 | for i = 1:size(rob,2) 114 | In{i} = Ic(:,:,i); 115 | end 116 | 117 | for i=1:N 118 | Mass{i}=simplify(m(i)*Jv{i}.'*Jv{i}+Jo{i}.'*R0{i}*In{i}*R0{i}.'*Jo{i}); 119 | end 120 | 121 | M=0; 122 | for i=1:N 123 | M=simplify(Mass{i}+M); 124 | end 125 | 126 | %% C 127 | for k=1:N 128 | for s=1:N 129 | c(1)=.5*((diff(M(k,s),q(1))+diff(M(k,1),q(s))-diff(M(1,s),q(k)))*dq(1)); 130 | for i=2:N 131 | c(i)=.5*((diff(M(k,s),q(i))+diff(M(k,i),q(s))-diff(M(i,s),q(k)))*dq(i))+c(i-1); 132 | end 133 | C(k,s)=simplify(c(N)); 134 | end 135 | end 136 | 137 | %% G 138 | P(1)=m(1)*[0,0,g]*poc{1}; 139 | for i=2:N 140 | P(i)=P(i-1)+m(i)*[0,0,g]*poc{i}; 141 | end 142 | P=simplify(P(N)); 143 | for i=1:N 144 | G(i,:)=simplify(diff(P,q(i))); 145 | end 146 | 147 | %% dynamic equations 148 | % M(q)*ddq + C(q,dq)dq + G(q) = u 149 | end -------------------------------------------------------------------------------- /InverseDynamic/MCG.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lsk-gith/robot_dynamic/0b3db06d0d6eeab48f959e7a19abff33984040fb/InverseDynamic/MCG.mat -------------------------------------------------------------------------------- /InverseDynamic/Newtown_InverseDynamics.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lsk-gith/robot_dynamic/0b3db06d0d6eeab48f959e7a19abff33984040fb/InverseDynamic/Newtown_InverseDynamics.m -------------------------------------------------------------------------------- /InverseDynamic/Result_L.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lsk-gith/robot_dynamic/0b3db06d0d6eeab48f959e7a19abff33984040fb/InverseDynamic/Result_L.mat -------------------------------------------------------------------------------- /InverseDynamic/RobotTool_Verify.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lsk-gith/robot_dynamic/0b3db06d0d6eeab48f959e7a19abff33984040fb/InverseDynamic/RobotTool_Verify.m -------------------------------------------------------------------------------- /InverseDynamic/readMe.txt: -------------------------------------------------------------------------------- 1 | excel表格文件组合值为：Angle（实际值） Velocity（实际值） Accelerate（实际值） q（理论值）, dq, （理论值） 2 | （1-6（列）） （7-12） （13-18） （19-24） （25-30） 3 | ddq（理论值） Current（实际值） CurrentFilt（实际值） Force（实际值） ForceFilt（实际值） 4 | （31-36） （37-42） （43-48） （49-54） （55-60） 5 | -------------------------------------------------------------------------------- /InverseDynamic/test.m: -------------------------------------------------------------------------------- 1 | clear 2 | clc 3 | %% set base data 4 | % alpha=[0, pi/2, 0, 0, pi/2, -pi/2]; 5 | % a= [0, 0, -0.264, -0.237, 0, 0]; 6 | % d= [0.144, 0, 0, 0.1065, 0.114, 0.0895]; 7 | % theta=[0, pi/2, 0, -pi/2, 0, 0]; 8 | 9 | %最新DHDH = 10 | % [0 0 0.1215 0; 11 | % 90 0 0 -90; 12 | % 0 -0.300 0 0; 13 | % 0 -0.276 0.1105 -90; 14 | % 90 0 0.090 0; 15 | % -90 0 0.082 0]; 16 | alpha=[0, pi/2, 0, 0, pi/2, -pi/2]; 17 | a= [0, 0, -300, -276, 0, 0].*0.001; 18 | d= [121.5, 0, 0, 110.5, 90, 82].*0.001; 19 | theta=[0, -pi/2, 0, -pi/2, 0, 0]; 20 | dh = [alpha; a; d; theta]; 21 | rob=[0,0,0,0,0,0];%0 standard for rotation, 1 standard for translation 22 | N = size(rob); 23 | g = sym('g'); 24 | assume(g,'real'); 25 | m = sym('m',N); 26 | assume(m, 'real') 27 | Pc = sym('Pc%d%d',[3,6]); 28 | assume(Pc,'real'); 29 | Ic = sym('Ic_%d%d_%d',[3,3,6]); 30 | assume(Ic,'real'); 31 | for i = 1:size(Ic,3) 32 | Ic(2,1,i)=Ic(1,2,i); 33 | Ic(3,1,i)=Ic(1,3,i); 34 | Ic(3,2,i)=Ic(2,3,i); 35 | end 36 | q = sym('q',N); % 1st group of state variables (joints' angle or position) 37 | assume(q, 'real') 38 | dq = sym('dq',N); % 2nd group of state variables (joints' velocity) 39 | assume(dq, 'real') 40 | 41 | % [J_L,M_L,C_L,G_L] = Lagrange_InverseDynamics(dh,rob,Pc,Ic,g,m,q,dq); 42 | % save ('Result_L.mat','J_L','M_L','C_L','G_L'); 43 | 44 | 45 | load('Result_L.mat'); 46 | mass_list = [2.920; 6.787; 2.450; 1.707; 1.707; 0.176]; 47 | %连杆质心相对于坐标系{i}坐标(m) 48 | mass_center_list = [0.0316 -3.1464 -13.8983; 49 | 131.5620 -0.0210 112.1840; 50 | 190.3840 0.0410 17.1800; 51 | 0.0886 21.0083 -2.5014; 52 | -0.0886 -21.0083 -2.5014; 53 | 0 0 8.0000]*10^-3; 54 | mass_center_list = mass_center_list'; 55 | %inertia_tensor_list 连杆相对于质心坐标系的惯量张量(kg*m^2) 56 | inertia_tensor_list = zeros(3,3,6); 57 | inertia_tensor_list(:,:,1) = [42.614 0.046 0.062; 0.046 41.164 -1.386; 0.062 -1.386 31.883]*10^-4; 58 | inertia_tensor_list(:,:,2) = [100.7 -1.8 1.6; -1.8 1100.8 0; 1.6 0 1087.1]*10^-4; 59 | inertia_tensor_list(:,:,3) = [31.45 0.48 7.23; 0.48 172.41 -0.15; 7.23 -0.15 166.82]*10^-4; 60 | inertia_tensor_list(:,:,4) = [20.92 -0.061 0.078; -0.061 16.808 0.992; 0.078 0.992 19.75]*10^-4; 61 | inertia_tensor_list(:,:,5) = [20.92 -0.061 -0.078; -0.061 16.808 -0.992; -0.078 -0.992 19.75]*10^-4; 62 | inertia_tensor_list(:,:,6) = [0.9296 0 0; 0 0.9485 0; 0 0 1.5925]*10^-4; 63 | %% prepare substitute data 64 | sym_sub = []; 65 | num_sub = []; 66 | for i = 1:size(Ic,3) 67 | sym_sub=[sym_sub,m(i)]; 68 | num_sub=[num_sub,mass_list(i)]; 69 | for j = 1:size(Pc,1) 70 | sym_sub=[sym_sub,Pc(j,i)]; 71 | num_sub=[num_sub,mass_center_list(j,i)]; 72 | end 73 | for j = 1:size(Ic,1) 74 | for k = 1:size(Ic,2) 75 | sym_sub=[sym_sub,Ic(j,k,i)]; 76 | num_sub=[num_sub,inertia_tensor_list(j,k,i)]; 77 | end 78 | end 79 | end 80 | 81 | G = subs(G_L,[g,sym_sub],[9.81,num_sub]); 82 | M = subs(M_L,sym_sub,num_sub); 83 | C = subs(C_L,sym_sub,num_sub); 84 | J=subs(J_L,Pc,mass_center_list); 85 | save('MCG.mat','M','C','G','J','dh'); 86 | %% calculate tau 87 | thetalist = [0;pi/3;pi/3;-pi/6;pi/2;pi/2]; 88 | dthetalist = [pi/2;pi/2;pi/6;0;pi/2;pi/4]; 89 | ddthetalist = [0;pi/34;0;pi/5;0;0.34]; 90 | tau_L = (double(subs(M,q,(thetalist+theta')'))*ddthetalist + double(subs(C,[q,dq],[(thetalist+theta')',dthetalist']))*dthetalist + double(subs(G,q,(thetalist'+theta))))' 91 | f_tip = zeros(2,3); 92 | tau_N = (Newtown_InverseDynamics(thetalist, dthetalist, ddthetalist, 9.81,dh, mass_list', mass_center_list, inertia_tensor_list, f_tip))' 93 | tau_tool = RobotTool_Verify(thetalist,dthetalist,ddthetalist,dh,mass_list',mass_center_list,inertia_tensor_list) 94 | 95 | 96 | % i = 1;%%第几组数据(1,2,0 分别代表第一次第二次第三次数据) 97 | % A=xlsread('Traj_15s_loop3_1',['Sheet',num2str(i)]); 98 | % thetalist = A(1:10:end,1:6)';%这里所有的量都应该是N*6的形式 99 | % dthetalist = A(1:10:end,7:12)'; 100 | % ddthetalist = A(1:10:end,13:18)'; 101 | % 102 | % tau_L=[]; 103 | % tau_N=[]; 104 | % tau_tool=[]; 105 | % for i=1:size(ddthetalist,2) 106 | % tau_L(i,:) = (double(subs(M,q,(thetalist(:,i)+theta')'))*ddthetalist(:,i) + double(subs(C,[q,dq],[(thetalist(:,i)+theta')',dthetalist(:,i)']))*dthetalist(:,i) + double(subs(G,q,(thetalist(:,i)'+theta))))'; 107 | % f_tip = zeros(2,3); 108 | % tau_N(i,:) = (Newtown_InverseDynamics(thetalist(:,i), dthetalist(:,i), ddthetalist(:,i), 9.81,dh, mass_list', mass_center_list, inertia_tensor_list, f_tip))'; 109 | % tau_tool(i,:) = RobotTool_Verify(thetalist(:,i),dthetalist(:,i),ddthetalist(:,i),dh,mass_list',mass_center_list,inertia_tensor_list); 110 | % end 111 | -------------------------------------------------------------------------------- /analysis_process_new_euler/M&C&G.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lsk-gith/robot_dynamic/0b3db06d0d6eeab48f959e7a19abff33984040fb/analysis_process_new_euler/M&C&G.mat -------------------------------------------------------------------------------- /analysis_process_new_euler/Traj_10s_11.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lsk-gith/robot_dynamic/0b3db06d0d6eeab48f959e7a19abff33984040fb/analysis_process_new_euler/Traj_10s_11.mat -------------------------------------------------------------------------------- /analysis_process_new_euler/calculteCCandKK.m: -------------------------------------------------------------------------------- 1 | function [CC]=calculteCCandKK(dh,Pc,m,Ic,g,R,P,G) 2 | ddq=[0,0,0,0,0,0]; 3 | dq=[0,0,0,0,0,0]; 4 | for i=1:6 5 | dq(i)=1; 6 | [F,N] = outsideEquation(dh,dq,ddq,Pc,m,Ic,g,R,P); 7 | [CG]= insideEquation(dh,F,N,Pc,R,P); 8 | CG = CG'; 9 | CG = CG - G; 10 | for j=1:6 11 | CC(j,i)=CG(j); 12 | dq(i)=0; 13 | end 14 | end 15 | end -------------------------------------------------------------------------------- /analysis_process_new_euler/calculteGandM.m: -------------------------------------------------------------------------------- 1 | function [G,M]=calculteGandM(dh,Pc,m,Ic,g,R,P) 2 | dq=[0,0,0,0,0,0]'; 3 | ddq=[0,0,0,0,0,0]'; 4 | [F,N] = outsideEquation(dh,dq,ddq,Pc,m,Ic,g,R,P); 5 | [G]= insideEquation(dh,F,N,Pc,R,P); 6 | G = G'; 7 | for i=1:6 8 | ddq(i)=1; 9 | [F,N] = outsideEquation(dh,dq,ddq,Pc,m,Ic,g,R,P); 10 | [MG]= insideEquation(dh,F,N,Pc,R,P); 11 | MG = MG'; 12 | MG = MG - G; 13 | for j=1:6 14 | M(j,i)=MG(j); 15 | ddq(i)=0; 16 | end 17 | end 18 | end -------------------------------------------------------------------------------- /analysis_process_new_euler/calculteKK.m: -------------------------------------------------------------------------------- 1 | function [KK]=calculteKK(dh,G,Pc,m,Ic,g,R,P,CC) 2 | % global nn ll; 3 | ddq=[0,0,0,0,0,0]; 4 | dq = [0,0,0,0,0,0]; 5 | if isa(m(1),'double') 6 | KK = []; 7 | end 8 | i = 1; 9 | for nn = 1:5 10 | for ll=(nn+1):6 11 | dq(nn)=1;dq(ll)=1; 12 | [F,N] = outsideEquation(dh,dq,ddq,Pc,m,Ic,g,R,P); 13 | [CK]= insideEquation(dh,F,N,Pc,R,P); 14 | CK=CK'; 15 | CK=CK-G-CC(:,nn)-CC(:,ll); 16 | KK(:,i)= CK; 17 | dq(nn)=0;dq(ll)=0; 18 | i = i + 1; 19 | end 20 | end 21 | end -------------------------------------------------------------------------------- /analysis_process_new_euler/compute_frame_transform.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lsk-gith/robot_dynamic/0b3db06d0d6eeab48f959e7a19abff33984040fb/analysis_process_new_euler/compute_frame_transform.m -------------------------------------------------------------------------------- /analysis_process_new_euler/insideEquation.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lsk-gith/robot_dynamic/0b3db06d0d6eeab48f959e7a19abff33984040fb/analysis_process_new_euler/insideEquation.m -------------------------------------------------------------------------------- /analysis_process_new_euler/new.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lsk-gith/robot_dynamic/0b3db06d0d6eeab48f959e7a19abff33984040fb/analysis_process_new_euler/new.m -------------------------------------------------------------------------------- /analysis_process_new_euler/outsideEquation.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lsk-gith/robot_dynamic/0b3db06d0d6eeab48f959e7a19abff33984040fb/analysis_process_new_euler/outsideEquation.m -------------------------------------------------------------------------------- /analysis_process_new_euler/解析结果/CC&KK.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lsk-gith/robot_dynamic/0b3db06d0d6eeab48f959e7a19abff33984040fb/analysis_process_new_euler/解析结果/CC&KK.mat -------------------------------------------------------------------------------- /analysis_process_new_euler/解析结果/M&C&G.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lsk-gith/robot_dynamic/0b3db06d0d6eeab48f959e7a19abff33984040fb/analysis_process_new_euler/解析结果/M&C&G.mat -------------------------------------------------------------------------------- /analysis_process_new_euler/解析结果/tau.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lsk-gith/robot_dynamic/0b3db06d0d6eeab48f959e7a19abff33984040fb/analysis_process_new_euler/解析结果/tau.mat -------------------------------------------------------------------------------- /readMe.txt: -------------------------------------------------------------------------------- 1 | 文档地址：https://www.jianshu.com/p/8947670a99e9 --------------------------------------------------------------------------------