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)
评论11