forked from petercorke/robotics-toolbox-matlab
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathmdl_puma560.m
153 lines (136 loc) · 4.89 KB
/
mdl_puma560.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
%MDL_PUMA560 Create model of Puma 560 manipulator
%
% MDL_PUMA560 is a script that creates the workspace variable p560 which
% describes the kinematic and dynamic characteristics of a Unimation Puma
% 560 manipulator using standard DH conventions.
%
% Also define the workspace vectors:
% qz zero joint angle configuration
% qr vertical 'READY' configuration
% qstretch arm is stretched out in the X direction
% qn arm is at a nominal non-singular configuration
%
% Notes::
% - SI units are used.
% - The model includes armature inertia and gear ratios.
%
% Reference::
% - "A search for consensus among model parameters reported for the PUMA 560 robot",
% P. Corke and B. Armstrong-Helouvry,
% Proc. IEEE Int. Conf. Robotics and Automation, (San Diego),
% pp. 1608-1613, May 1994.
%
% See also SerialRevolute, mdl_puma560akb, mdl_stanford.
% MODEL: Unimation, Puma560, dynamics, 6DOF, standard_DH
%
% 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-2015, 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 distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB. If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com
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 );
p560 = SerialLink(L, 'name', 'Puma 560', ...
'manufacturer', 'Unimation', 'ikine', 'puma', 'comment', 'viscous friction; params of 8/95');
p560.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];
clear L