FKine_fkine_orderbkb_机器人正运动学建模_DH_

clear ; clc; close all; % 机器人各连杆参数值 d1 = 0; d2 = 0; d3 = 0; d4 = 740; d5 = 0; d6 = 100; a1 = 150; a2 = 770; a3 = 100; a4 = 0; a5 = 0; a6 = 0; alpha1 = 90 / 180 * pi; alpha2 = 0 / 180 * pi; alpha3 = 90 / 180 * pi; alpha4 = -90 / 180 * pi; alpha5 = 90 / 180 * pi; alpha6 = 0 / 180 * pi; L(1)=Link([0 d1 a1 alpha1]); L(1).qlim=[-pi,pi]; L(2)=Link([90/180*pi d2 a2 alpha2]); L(2).qlim=[-pi,pi]; L(3)=Link([0 d3 a3 alpha3]); L(3).qlim=[-pi,pi]; L(4)=Link([0 d4 a4 alpha4]); L(4).qlim=[-pi,pi]; L(5)=Link([0 d5 a5 alpha5]); L(5).qlim=[-pi,pi]; L(6)=Link([0 d6 a6 alpha6]); L(6).qlim=[-pi,pi]; ARC=SerialLink(L,'name','ARC'); %% 正运动学验证 % theta1 = [pi/6 0 -pi/3 0 -pi/6 pi/6]; % T = forwardkinamicsDH(theta1); % [r,p,y] = dcm2angle(T(1:3,1:3),'ZYX'); % r = r/pi*180 % p = p/pi*180 % y = y/pi*180 % position = T(1:3,4) % theta1(2) = theta1(2)+pi/2;%由于关节2的初始角度为90°，因此在工具箱中角度要加上 % % g = ARC.fkine(theta1); % ARC.teach(theta1); %% 逆运动学验证 % T = eye(4); % r = 10/180*pi; % p = 80/180*pi; % y = 10/180*pi; % position =[200 200 600]'; % T(1:3,1:3) = angle2dcm(r,p,y,'ZYX'); % T(1:3,4) = position; % joint = inversekinamicsDH(T); % joint(1,2) = joint(1,2)+pi/2; % ARC.teach(joint(1,:)); %% 运动仿真 for i = 1:4 T{i} = eye(4); T{i}(1:3,1:3) = roty(180); end position = [1020 0 -1060]'; T{1}(1:3,4) = position; position(1) = position(1)-300; T{2}(1:3,4) = position; position(2) = position(2)-700; T{3}(1:3,4) = position; position(3) = position(3)+640; T{4}(1:3,4) = position; for i = 1:4 t(i) = SE3(T{i}); end q0 =[0,-1.87+pi/2,0.343,-3.14,0.0457,0]; time = 0:0.1:2; T0_1 = ctraj(t(1),t(2),length(time)); Q0_1 = ARC.ikine(T0_1,'q0',q0,'ilimit',1500,'tol',1e-4); ARC.plot(Q0_1) T1_2 = ctraj(t(2),t(3),length(time)); Q1_2 = ARC.ikine(T1_2,'q0',Q0_1(21,:),'ilimit',1500,'tol',1e-4); ARC.plot(Q1_2) T2_3 = ctraj(t(3),t(4),length(time)); Q2_3 = ARC.ikine(T2_3,'q0',Q1_2(21,:),'ilimit',1500,'tol',1e-4); ARC.plot(Q2_3)