%FANUC机器人系统模型建立
%说明:
% L10,L11,L2,L30,L31,L4 为手臂长度,可以修改;
% a1,a2,a3,a4,a5,a6为机器人的6个关节角度,可以修改.
%作者:李引魁
%日期:2009/04/25
%版本:V0.1
%机器人手臂参数
%下边的手臂长度可修改
L10 = 150;
L11 = 50 ;
L2 = 150;
L30 = 50 ;
L31 = 100;
L4 = 50 ;
%各关节角度
%关节角度可修改
a1 = 0 * pi /180;
a2 = 0 * pi /180;
a3 = 0 * pi /180;
a4 = 0 * pi /180;
a5 = 0 * pi /180;
a6 = 0 * pi /180;
%机器人空间位置绘制并显示
s1 = sin(a1);
c1 = cos(a1);
s2 = sin(a2);
c2 = cos(a2);
s3 = sin(a3 - pi/2);
c3 = cos(a3 - pi/2);
s31 = sin(pi/2);
c31 = cos(pi/2);
s4 = sin(a4);
c4 = cos(a4);
s5 = sin(a5);
c5 = cos(a5);
s6 = sin(a6);
c6 = cos(a6);
%运动方程建立
%关节1 A点
A01 = sym('[1, 0, 0, 0; 0, 1, 0, 0; 0, 0, 1, L10; 0, 0, 0, 1]');
T01 = A01 ;
%关节2 B点
R01 = sym('[c1, -s1, 0, 0; s1, c1, 0, 0; 0, 0, 1, 0; 0, 0, 0, 1]');
A12 = sym('[1, 0, 0, L11; 0, 1, 0, 0; 0, 0, 1, 0; 0, 0, 0, 1]');
T02 = T01 * R01 * A12 ;
%关节3 C点
R21 = sym('[c2, 0, s2, 0; 0, 1, 0, 0; -s2, 0, c2, 0; 0, 0, 0, 1]');
A23 = sym('[1, 0, 0, 0; 0, 1, 0, 0; 0, 0, 1, L2; 0, 0, 0, 1]');
T03 = T02 * R21 * A23 ;
%关节4 E点
R32 = sym('[c3, 0, s3, 0; 0, 1, 0, 0; -s3, 0, c3, 0; 0, 0, 0, 1]');
A34 = sym('[1, 0, 0, 0; 0, 1, 0, 0; 0, 0, 1, L30; 0, 0, 0, 1]');
R321= sym('[c31, 0, s31, 0; 0, 1, 0, 0; -s31, 0, c31, 0; 0, 0, 0, 1]');
A341= sym('[1, 0, 0, 0; 0, 1, 0, 0; 0, 0, 1, L31; 0, 0, 0, 1]');
T04 = T03 * R32 * A34 * R321 * A341 ;
%P点坐标
R34 = sym('[c4, -s4, 0, 0; s4, c4, 0, 0; 0, 0, 1, 0; 0, 0, 0, 1]');
R45 = sym('[c5, 0, s5, 0; 0, 1, 0, 0; -s5, 0, c5, 0; 0, 0, 0, 1]');
R56 = sym('[c6, -s6, 0, 0; s6, c6, 0, 0; 0, 0, 1, 0; 0, 0, 0, 1]');
A4P = sym('[1, 0, 0, 0; 0, 1, 0, 0; 0, 0, 1, L4; 0, 0, 0, 1]');
T0P = T04 * R34 * R45 * R56 * A4P ;
%各关节位置
T01 = eval(T01);
T02 = eval(T02);
T03 = eval(T03);
T04 = eval(T04);
T0P = eval(T0P);
%机械手结构坐标
T0 = [0,0,0,1]';
Z01 = T01(:,4);
Z02 = T02(:,4);
Z03 = T03(:,4);
Z04 = T04(:,4);
Z0P = T0P(:,4);
W = [T0,Z01,Z02,Z03,Z04,Z0P];
figure(1)
xlim([-200,200])
ylim([-200,200])
zlim([ 0,500])
hold on
view([0.5,1,1])
%机械手坐标提取，图形绘制
plot3(W(1,:),W(2,:),W(3,:),'linewidth',3);
xlabel('X');
ylabel('Y');
zlabel('Z');
grid on
%机械手末端位姿
T0P
