Home > . > puma560.m

puma560

PURPOSE ^

PUMA560 Load kinematic and dynamic data for a Puma 560 manipulator

SYNOPSIS ^

This is a script file.

DESCRIPTION ^

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.

CROSS-REFERENCE INFORMATION ^

This function calls: This function is called by:

SOURCE CODE ^

0001 %PUMA560 Load kinematic and dynamic data for a Puma 560 manipulator
0002 %
0003 %    PUMA560
0004 %
0005 % Defines the object 'p560' in the current workspace which describes the
0006 % kinematic and dynamic characterstics of a Unimation Puma 560 manipulator
0007 % using standard DH conventions.
0008 % The model includes armature inertia and gear ratios.
0009 %
0010 % Also define the vector qz which corresponds to the zero joint
0011 % angle configuration, qr which is the vertical 'READY' configuration,
0012 % and qstretch in which the arm is stretched out in the X direction.
0013 %
0014 % See also: ROBOT, PUMA560AKB, STANFORD, TWOLINK.
0015 
0016 %
0017 % Notes:
0018 %    - the value of m1 is given as 0 here.  Armstrong found no value for it
0019 % and it does not appear in the equation for tau1 after the substituion
0020 % is made to inertia about link frame rather than COG frame.
0021 % updated:
0022 % 2/8/95  changed D3 to 150.05mm which is closer to data from Lee, AKB86 and Tarn
0023 %  fixed errors in COG for links 2 and 3
0024 % 29/1/91 to agree with data from Armstrong etal.  Due to their use
0025 %  of modified D&H params, some of the offsets Ai, Di are
0026 %  offset, and for links 3-5 swap Y and Z axes.
0027 % 14/2/91 to use Paul's value of link twist (alpha) to be consistant
0028 %  with ARCL.  This is the -ve of Lee's values, which means the
0029 %  zero angle position is a righty for Paul, and lefty for Lee.
0030 %  Note that gravity load torque is the motor torque necessary
0031 %  to keep the joint static, and is thus -ve of the gravity
0032 %  caused torque.
0033 %
0034 % 8/95 fix bugs in COG data for Puma 560. This led to signficant errors in
0035 %  inertia of joint 1.
0036 % $Log: not supported by cvs2svn $
0037 % Revision 1.4  2008/04/27 11:36:54  cor134
0038 % Add nominal (non singular) pose qn
0039 
0040 % Copyright (C) 1993-2008, by Peter I. Corke
0041 %
0042 % This file is part of The Robotics Toolbox for Matlab (RTB).
0043 %
0044 % RTB is free software: you can redistribute it and/or modify
0045 % it under the terms of the GNU Lesser General Public License as published by
0046 % the Free Software Foundation, either version 3 of the License, or
0047 % (at your option) any later version.
0048 %
0049 % RTB is distributed in the hope that it will be useful,
0050 % but WITHOUT ANY WARRANTY; without even the implied warranty of
0051 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
0052 % GNU Lesser General Public License for more details.
0053 %
0054 % You should have received a copy of the GNU Leser General Public License
0055 % along with RTB.  If not, see <http://www.gnu.org/licenses/>.
0056 
0057 clear L
0058 L{1} = link([ pi/2 0    0    0    0], 'standard');
0059 L{2} = link([ 0     .4318    0    0    0], 'standard');
0060 L{3} = link([-pi/2 .0203    0    .15005    0], 'standard');
0061 L{4} = link([pi/2 0    0    .4318    0], 'standard');
0062 L{5} = link([-pi/2 0    0    0    0], 'standard');
0063 L{6} = link([0     0    0    0    0], 'standard');
0064 
0065 L{1}.m = 0;
0066 L{2}.m = 17.4;
0067 L{3}.m = 4.8;
0068 L{4}.m = 0.82;
0069 L{5}.m = 0.34;
0070 L{6}.m = .09;
0071 
0072 L{1}.r = [ 0    0       0 ];
0073 L{2}.r = [ -.3638  .006    .2275];
0074 L{3}.r = [ -.0203  -.0141  .070];
0075 L{4}.r = [ 0    .019    0];
0076 L{5}.r = [ 0    0       0];
0077 L{6}.r = [ 0    0       .032];
0078 
0079 L{1}.I = [  0     0.35     0     0     0     0];
0080 L{2}.I = [  .13     .524     .539     0     0     0];
0081 L{3}.I = [   .066  .086     .0125   0     0     0];
0082 L{4}.I = [  1.8e-3  1.3e-3  1.8e-3  0     0     0];
0083 L{5}.I = [  .3e-3   .4e-3   .3e-3   0     0     0];
0084 L{6}.I = [  .15e-3  .15e-3  .04e-3  0     0     0];
0085 
0086 L{1}.Jm =  200e-6;
0087 L{2}.Jm =  200e-6;
0088 L{3}.Jm =  200e-6;
0089 L{4}.Jm =  33e-6;
0090 L{5}.Jm =  33e-6;
0091 L{6}.Jm =  33e-6;
0092 
0093 L{1}.G =  -62.6111;
0094 L{2}.G =  107.815;
0095 L{3}.G =  -53.7063;
0096 L{4}.G =  76.0364;
0097 L{5}.G =  71.923;
0098 L{6}.G =  76.686;
0099 
0100 % viscous friction (motor referenced)
0101 L{1}.B =   1.48e-3;
0102 L{2}.B =   .817e-3;
0103 L{3}.B =    1.38e-3;
0104 L{4}.B =   71.2e-6;
0105 L{5}.B =   82.6e-6;
0106 L{6}.B =   36.7e-6;
0107 
0108 % Coulomb friction (motor referenced)
0109 L{1}.Tc = [ .395    -.435];
0110 L{2}.Tc = [ .126    -.071];
0111 L{3}.Tc = [ .132    -.105];
0112 L{4}.Tc = [ 11.2e-3 -16.9e-3];
0113 L{5}.Tc = [ 9.26e-3 -14.5e-3];
0114 L{6}.Tc = [ 3.96e-3 -10.5e-3];
0115 
0116 
0117 %
0118 % some useful poses
0119 %
0120 qz = [0 0 0 0 0 0]; % zero angles, L shaped pose
0121 qr = [0 pi/2 -pi/2 0 0 0]; % ready pose, arm up
0122 qs = [0 0 -pi/2 0 0 0];
0123 qn=[0 pi/4 pi 0 pi/4  0];
0124 
0125 
0126 p560 = robot(L, 'Puma 560', 'Unimation', 'params of 8/95');
0127 clear L
0128 p560.name = 'Puma 560';
0129 p560.manuf = 'Unimation';

Generated on Sun 15-Feb-2009 18:09:29 by m2html © 2003