diff --git a/Identification_main.m b/Identification_main.m index 166d17c..06d078f 100644 --- a/Identification_main.m +++ b/Identification_main.m @@ -13,7 +13,7 @@ robot = get_robot_R1000(file,opt); robot = get_Kinematics(robot, opt); %TODO verify kinematics via robotics toolbox or other software result -R1000_Dynamics; +R1000_Dynamics_num; % opt.Isreal = false; % robot = get_velocity(robot, opt); % robot = get_regressor(robot,opt); diff --git a/R1000_Dynamics_num.m b/R1000_Dynamics_num.m index fa5b892..a514628 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 = [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_]'; +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_]'; % Get general mass matrix Glist=[]; @@ -29,10 +29,14 @@ 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)); + M = TransInv(T(:,:,i+1))*robot.T(:,:,i+1)*transl(com_pos(:,i+1)); end Mlist_CG = cat(3, Mlist_CG, M); end +for i = i:N + M = transl(com_pos(:,i)) + 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); @@ -62,9 +66,9 @@ J=sym(zeros(6,N)); exf=[0;0;0;0;0;0]; for i = 1:length(q_J) - [V1(:,:, i),Vd1(:,:, i),Adgab_mat(:,:,:,i),Fmat(:,:,i)] ... + [V1(:,:, i),Vd1(:,:, i),Adgab_mat(:,:,:,i),Fmat(:,:,i),taumat(:,i)] ... = InverseDynamics_debug(thetalist(i,:)', dthetalist(i,:)', ddthetalist(i,:)', ... - [0;0;-9.8], exf, Mlist_CG, Glist, Slist); + [0;0;-9.806], exf, Mlist_CG, Glist, Slist); 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 @@ -74,13 +78,24 @@ end % 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); hold on; %added minus, so should be the same as simpack - plot(time,-reshape(F_Simpack(1,i,:),[1,length(F_Simpack)])) + plot(time,taumat(i+6,:)) + xlabel('time(s)') + ylabel('Torque(Nm)') +% plot(SPCK_Result.Crv(i+4).x,SPCK_Result.Crv(i+4).y) +end + +F_Simpack = permute(F_Simpack,[2 1 3]); +figure(2) +for i = 1:3 + subplot(3,1,i); + hold on; + %added minus, so should be the same as simpack + plot(time,-reshape(F_Simpack(8,i,:),[1,length(F_Simpack)])) % plot(SPCK_Result.Crv(i+4).x,SPCK_Result.Crv(i+4).y) end diff --git a/get_robot_R1000.m b/get_robot_R1000.m index bc9d741..6ecb2ff 100644 --- a/get_robot_R1000.m +++ b/get_robot_R1000.m @@ -9,8 +9,8 @@ switch opt.robot_def case 'SDH' case 'MDH' robot.theta = [0,0,0,0,-pi/2,0,-pi/2,-pi/2,0]; - 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.a = [0,0.2,0.5,0.45,0.12,0,0,0, 0.05896]; + 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']; otherwise @@ -51,17 +51,17 @@ switch opt.robot_def 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]; + 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;0;1]; - axis_of_rot(:,3) = [0;0;1]; - axis_of_rot(:,4) = [0;0;1]; + axis_of_rot(:,2) = [0;-1;0]; + axis_of_rot(:,3) = [0;-1;0]; + axis_of_rot(:,4) = [0;-1;0]; axis_of_rot(:,5) = [0;0;1]; - axis_of_rot(:,6) = [0;0;1]; - axis_of_rot(:,7) = [0;0;1]; - axis_of_rot(:,8) = [0;0;1]; - axis_of_rot(:,9) = [0;0;1]; + axis_of_rot(:,6) = [1;0;0]; + axis_of_rot(:,7) = [0;0;-1]; + axis_of_rot(:,8) = [0;-1;0]; + axis_of_rot(:,9) = [0;0;0]; % 画图 % com_pos(:,1) = [0;0;0.122]; % com_pos(:,2) = [0.373;0;0]; @@ -76,12 +76,12 @@ switch opt.robot_def 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(:,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) = [-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]; + 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; % 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]); @@ -114,10 +114,12 @@ switch opt.robot_def 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]); + link_inertia(:,:,8) = [[1.5162872e+03 -4.6245133e+01 4.4556929e+03];... + [-4.6245133e+01 5.9901606e+04 6.0350548e+0];... + [4.4556929e+03 6.0350548e+0 5.8819998e+04]]*10^-6; + link_inertia(:,:,9) = [[1.1730106e+03 3.3834506e+0 4.6097678e+01];... + [3.3834506e+0 1.7996852e+03 5.2088778e+0];... + [4.6097678e+01 5.2088778e+0 1.3960734e+03]]*10^-6; % verify if link_inertia is issymmetric for i = 1:ndof if(issymmetric(link_inertia(:,:,i))==false)