基于matlab的机器手puma560的运行仿真

%PUMA560 Load kinematic and dynamic data for a Puma 560 manipulator
%
%	PUMA560
%
% Defines the object ‘p560‘ in the current workspace which describes the 
% kinematic and dynamic characterstics of a Unimation Puma 560 manipulator
% using standard DH conventions.
% The model includes armature inertia and gear ratios.
%
% Also define the vector qz which corresponds to the zero joint
% angle configuration qr which is the vertical ‘READY‘ configuration
% and qstretch in which the arm is stretched out in the X direction.
%
% See also: ROBOT PUMA560AKB STANFORD TWOlink.

%
% Notes:
%    - the value of m1 is given as 0 here.  Armstrong found no value for it
% and it does not appear in the equation for tau1 after the substituion
% is made to inertia about link frame rather than COG frame.
% updated:
% 2/8/95  changed D3 to 150.05mm which is closer to data from Lee AKB86 and Tarn
%  fixed errors in COG for links 2 and 3
% 29/1/91 to agree with data from Armstrong etal.  Due to their use
%  of modified D&H params some of the offsets Ai Di are
%  offset and for links 3-5 swap Y and Z axes.
% 14/2/91 to use Paul‘s value of link twist （alpha） to be consistant
%  with ARCL.  This is the -ve of Lee‘s values which means the
%  zero angle position is a righty for Paul and lefty for Lee.
%  Note that gravity load torque is the motor torque necessary
%  to keep the joint static and is thus -ve of the gravity
%  caused torque.
%
% 8/95 fix bugs in COG data for Puma 560. This led to signficant errors in
%  inertia of joint 1. 
% $Log: not supported by cvs2svn $
% Revision 1.4  2008/04/27 11:36:54  cor134
% Add nominal （non singular） pose qn

% Copyright （C） 1993-2008 by Peter I. Corke
%
% This file is part of The Robotics Toolbox for Matlab （RTB）.
% 
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation either version 3 of the License or
% （at your option） any later version.
% 
% RTB is distribu