六自由度机械臂的建模与仿真，机器人的开发

function puma560=mdl_puma560（）


clear L
deg = pi/180;

% joint angle limits from 
% A combined optimization method for solving the inverse kinematics problem...
% Wang & Chen
% IEEE Trans. RA 7（4） 1991 pp 489-
L（1） = Revolute（‘d‘ 0 ‘a‘ 0 ‘alpha‘ pi/2 ...
    ‘I‘ [0 0.35 0 0 0 0] ...
    ‘r‘ [0 0 0] ...
    ‘m‘ 0 ...
    ‘Jm‘ 200e-6 ...
    ‘G‘ -62.6111 ...
    ‘B‘ 1.48e-3 ...
    ‘Tc‘ [0.395 -0.435] ...
    ‘qlim‘ [-160 160]*deg ）;

L（2） = Revolute（‘d‘ 0 ‘a‘ 0.4318 ‘alpha‘ 0 ...
    ‘I‘ [0.13 0.524 0.539 0 0 0] ...
    ‘r‘ [-0.3638 0.006 0.2275] ...
    ‘m‘ 17.4 ...
    ‘Jm‘ 200e-6 ...
    ‘G‘ 107.815 ...
    ‘B‘ .817e-3 ...
    ‘Tc‘ [0.126 -0.071] ...
    ‘qlim‘ [-45 225]*deg ）;

L（3） = Revolute（‘d‘ 0.15005 ‘a‘ 0.0203 ‘alpha‘ -pi/2  ...
    ‘I‘ [0.066 0.086 0.0125 0 0 0] ...
    ‘r‘ [-0.0203 -0.0141 0.070] ...
    ‘m‘ 4.8 ...
    ‘Jm‘ 200e-6 ...
    ‘G‘ -53.7063 ...
    ‘B‘ 1.38e-3 ...
    ‘Tc‘ [0.132 -0.105] ...
    ‘qlim‘ [-225 45]*deg ）;

L（4） = Revolute（‘d‘ 0.4318 ‘a‘ 0 ‘alpha‘ pi/2  ...
    ‘I‘ [1.8e-3 1.3e-3 1.8e-3 0 0 0] ...
    ‘r‘ [0 0.019 0] ...
    ‘m‘ 0.82 ...
    ‘Jm‘ 33e-6 ...
    ‘G‘ 76.0364 ...
    ‘B‘ 71.2e-6 ...
    ‘Tc‘ [11.2e-3 -16.9e-3] ...
    ‘qlim‘ [-110 170]*deg）;

L（5） = Revolute（‘d‘ 0 ‘a‘ 0 ‘alpha‘ -pi/2  ...
    ‘I‘ [0.3e-3 0.4e-3 0.3e-3 0 0 0] ...
    ‘r‘ [0 0 0] ...
    ‘m‘ 0.34 ...
    ‘Jm‘ 33e-6 ...
    ‘G‘ 71.923 ...
    ‘B‘ 82.6e-6 ...
    ‘Tc‘ [9.26e-3 -14.5e-3] ...
    ‘qlim‘ [-100 100]*deg ）;


L（6） = Revolute（‘d‘ 0 ‘a‘ 0 ‘alpha‘ 0  ...
    ‘I‘ [0.15e-3 0.15e-3 0.04e-3 0 0 0] ...
    ‘r‘ [0 0 0.032] ...
    ‘m‘ 0.09 ...
    ‘Jm‘ 33e-6 ...
    ‘G‘ 76.686 ...
    ‘B‘ 36.7e-6 ...
    ‘Tc‘ [3.96e-3 -10.5e-3] ...
    ‘qlim‘ [-266 266]*deg ）;

puma560 = Seriallink（L ‘name‘ ‘Puma 560‘ ...
    ‘manufacturer‘ ‘Unimation‘ ‘ikine‘ ‘puma‘ ‘comment‘ ‘viscous friction; params of 8/95‘）;


puma560.model3d = ‘UNIMATE/puma560‘;

%
% some useful poses
%
qz = [0 0 0 0 0 0]; % zero angles L shaped pose
qr = [0 pi/2 -pi/2 0 0 0]; % ready pose arm up
qs = [0 0 -pi/2 0 0 0];
qn=[0 pi/4 pi 0 pi/4  0];
hold o