0001
0002
0003
0004
0005
0006
0007
0008
0009
0010
0011
0012
0013
0014
0015
0016
0017
0018
0019
0020
0021
0022
0023
0024
0025
0026
0027
0028
0029
0030
0031
0032
0033
0034
0035 function theta = ikine560(robot, T,configuration)
0036
0037 if robot.n ~= 6,
0038 error('Solution only applicable for 6DOF manipulator');
0039 end
0040
0041 if robot.mdh ~= 0,
0042 error('Solution only applicable for standard DH conventions');
0043 end
0044
0045 if ndims(T) == 3,
0046 theta = [];
0047 for k=1:size(T,3),
0048 if nargin < 3,
0049 theta = [theta; ikine560(robot, T(:,:,k))];
0050 else
0051 theta = [theta; ikine560(robot, T(:,:,k), configuration)];
0052 end
0053 end
0054
0055 return;
0056 end
0057 L = robot.links;
0058 a1 = L{1}.A;
0059 a2 = L{2}.A;
0060 a3 = L{3}.A;
0061
0062 if ~isempty( find( [L{4}.A L{5}.A L{6}.A] ~= 0 ))
0063 error('wrist is not spherical')
0064 end
0065
0066 d1 = L{1}.D;
0067 d2 = L{2}.D;
0068 d3 = L{3}.D;
0069 d4 = L{4}.D;
0070
0071 if ~ishomog(T),
0072 error('T is not a homog xform');
0073 end
0074
0075
0076 T = inv(robot.base) * T;
0077
0078
0079
0080
0081 Ox = T(1,2);
0082 Oy = T(2,2);
0083 Oz = T(3,2);
0084
0085 Ax = T(1,3);
0086 Ay = T(2,3);
0087 Az = T(3,3);
0088
0089 Px = T(1,4);
0090 Py = T(2,4);
0091 Pz = T(3,4);
0092
0093
0094
0095
0096 if nargin < 3,
0097 configuration = '';
0098 else
0099 configuration = lower(configuration);
0100 end
0101
0102
0103
0104 n1 = -1;
0105 n2 = -1;
0106 n4 = -1;
0107 if ~isempty(findstr(configuration, 'l')),
0108 n1 = -1;
0109 end
0110 if ~isempty(findstr(configuration, 'r')),
0111 n1 = 1;
0112 end
0113 if ~isempty(findstr(configuration, 'u')),
0114 if n1 == 1,
0115 n2 = 1;
0116 else
0117 n2 = -1;
0118 end
0119 end
0120 if ~isempty(findstr(configuration, 'd')),
0121 if n1 == 1,
0122 n2 = -1;
0123 else
0124 n2 = 1;
0125 end
0126 end
0127 if ~isempty(findstr(configuration, 'n')),
0128 n4 = 1;
0129 end
0130 if ~isempty(findstr(configuration, 'f')),
0131 n4 = -1;
0132 end
0133
0134
0135
0136
0137
0138
0139
0140
0141
0142
0143 r=sqrt(Px^2 + Py^2);
0144 if (n1 == 1),
0145 theta(1)= atan2(Py,Px) + asin(d3/r);
0146 else
0147 theta(1)= atan2(Py,Px) + pi - asin(d3/r);
0148 end
0149
0150
0151
0152
0153
0154
0155
0156
0157
0158
0159
0160 V114= Px*cos(theta(1)) + Py*sin(theta(1));
0161 r=sqrt(V114^2 + Pz^2);
0162 Psi = acos((a2^2-d4^2-a3^2+V114^2+Pz^2)/(2.0*a2*r));
0163 if ~isreal(Psi),
0164 error('point not reachable');
0165 end
0166 theta(2) = atan2(Pz,V114) + n2*Psi;
0167
0168
0169
0170
0171
0172
0173
0174 num = cos(theta(2))*V114+sin(theta(2))*Pz-a2;
0175 den = cos(theta(2))*Pz - sin(theta(2))*V114;
0176 theta(3) = atan2(a3,d4) - atan2(num, den);
0177
0178
0179
0180
0181
0182
0183
0184
0185
0186
0187
0188 V113 = cos(theta(1))*Ax + sin(theta(1))*Ay;
0189 V323 = cos(theta(1))*Ay - sin(theta(1))*Ax;
0190 V313 = cos(theta(2)+theta(3))*V113 + sin(theta(2)+theta(3))*Az;
0191 theta(4) = atan2((n4*V323),(n4*V313));
0192
0193
0194
0195
0196
0197
0198
0199
0200
0201
0202 num = -cos(theta(4))*V313 - V323*sin(theta(4));
0203 den = -V113*sin(theta(2)+theta(3)) + Az*cos(theta(2)+theta(3));
0204 theta(5) = atan2(num,den);
0205
0206
0207
0208
0209
0210
0211
0212
0213
0214
0215
0216
0217
0218
0219
0220
0221 V112 = cos(theta(1))*Ox + sin(theta(1))*Oy;
0222 V132 = sin(theta(1))*Ox - cos(theta(1))*Oy;
0223 V312 = V112*cos(theta(2)+theta(3)) + Oz*sin(theta(2)+theta(3));
0224 V332 = -V112*sin(theta(2)+theta(3)) + Oz*cos(theta(2)+theta(3));
0225 V412 = V312*cos(theta(4)) - V132*sin(theta(4));
0226 V432 = V312*sin(theta(4)) + V132*cos(theta(4));
0227 num = -V412*cos(theta(5)) - V332*sin(theta(5));
0228 den = - V432;
0229 theta(6) = atan2(num,den);
0230