task1_forward_model.asv

% make a instance of Parameters_Class
Robot = IRB460();
% get public parameters
para = Robot.get_parameters();
angle1L = para.angle1L;
angle1R = para.angle1R;
angle2L = para.angle2L;
angle2R = para.angle2R;
angle3L = para.angle3L;
angle3R = para.angle3R;
dG1 = para.dG1;
a1I = para.a1I;
d1I = para.d1I;
alphaI2 = para.alphaI2;
thetaI2 = para.thetaI2;
a23 = para.a23;
theta23 = para.theta23;
a34 = para.a34;
alpha46 = para.alpha46;
a46 = para.a46; 
d46 = para.d46;

% establish T from alpha, a, d, theta
syms alpha a d theta
T = [cos(theta), -sin(theta), 0, a;
    sin(theta)*cos(alpha), cos(theta)*cos(alpha), -sin(alpha), -sin(alpha)*d;
    sin(theta)*sin(alpha), cos(theta)*sin(alpha), cos(alpha), cos(alpha)*d;
    0, 0, 0, 1];

% establish Ts from T, parameters, angle1, angle2, angle3.
syms angle1 angle2 angle3
TG1 = subs(T, [alpha, a, d, theta], [0, 0, dG1, 0+angle1]);
T1I = subs(T, [alpha, a, d, theta], [0, a1I, d1I, 0]);
TI2 = subs(T, [alpha, a, d, theta], [alphaI2, 0, 0, thetaI2+angle2]);
T23 = subs(T, [alpha, a, d, theta], [0, a23, 0, theta23+angle3-angle2]);
T34 = subs(T, [alpha, a, d, theta], [0, a34, 0, -angle3]);
T46 = subs(T, [alpha, a, d, theta], [alpha46, a46, d46, 0]);

TG6 = TG1 * T1I * TI2 * T23 * T34 * T46;
simplify(TG6);

TG6 = TG1 * T1I * TI2 * T23 * T34 * T46;
simplify(TG6);


%通过改进D-H参数建立连杆，那个参数为变量就不写那个参数。
% L(1) = Link([0+angle1               dG1   0    0       ], 'qlim', [angle1L,angle1R], 'modified');
% L(I) = Link([0                      d1I   a1I  0       ],                            'modified');
% L(2) = Link([thetaI2+angle2         0     0    alphaI2 ], 'qlim', [angle2L,angle2R], 'modified');
% L(3) = Link([theta23+angle3-angle2  0     0    0       ], 'qlim', [angle3L,angle3R], 'modified');
% L(4) = Link([-angle3                0     0    0       ],                            'modified');
% L(6) = Link([0                      d46   a46  alpha46 ],                            'modified');

%编号只能用数字，上下L的编号以上面为准
L(1) = Link([0+angle1               dG1   0    0       ], 'qlim', [angle1L,angle1R], 'modified');
L(2) = Link([0                      d1I   a1I  0       ],                            'modified');
L(3) = Link([thetaI2+angle2         0     0    alphaI2 ], 'qlim', [angle2L,angle2R], 'modified');
L(4) = Link([theta23+angle3-angle2  0     0    0       ], 'qlim', [angle3L,angle3R], 'modified');
L(5) = Link([-angle3                0     0    0       ],                            'modified');
L(6) = Link([0                      d46   a46  alpha46 ],                            'modified');
IRB460 = SerialLink([L(1),]);   %建立模型
IRB460.display
IRB460.teach;       %画出模型并进行调控