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