00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012 #include <math.h>
00013 #include "posemath.h"
00014 #include "puma560kins.h"
00015 #include "emcmot.h"
00016
00017
00018 double PUMA560_A2 = 300.0;
00019 double PUMA560_A3 = 50.0;
00020 double PUMA560_D3 = 70.0;
00021 double PUMA560_D4 = 400.0;
00022
00023 int kinematicsForward(const double * joint,
00024 EmcPose * world,
00025 const KINEMATICS_FORWARD_FLAGS * fflags,
00026 KINEMATICS_INVERSE_FLAGS * iflags)
00027 {
00028
00029 double s1, s2, s3, s4, s5, s6;
00030 double c1, c2, c3, c4, c5, c6;
00031 double s23;
00032 double c23;
00033 double t1, t2, t3, t4, t5;
00034 double sumSq, k;
00035 PmHomogeneous hom;
00036 PmPose worldPose;
00037 PmRpy rpy;
00038
00039
00040 s1 = sin(joint[0]);
00041 s2 = sin(joint[1]);
00042 s3 = sin(joint[2]);
00043 s4 = sin(joint[3]);
00044 s5 = sin(joint[4]);
00045 s6 = sin(joint[5]);
00046
00047
00048 c1 = cos(joint[0]);
00049 c2 = cos(joint[1]);
00050 c3 = cos(joint[2]);
00051 c4 = cos(joint[3]);
00052 c5 = cos(joint[4]);
00053 c6 = cos(joint[5]);
00054
00055 s23 = c2 * s3 + s2 * c3;
00056 c23 = c2 * c3 - s2 * s3;
00057
00058
00059
00060 t1 = c4 * c5 * c6 - s4 * s6;
00061 t2 = s23 * s5 * c6;
00062 t3 = s4 * c5 * c6 + c4 * s6;
00063 t4 = c23 * t1 - t2;
00064 t5 = c23 * s5 * c6;
00065
00066
00067 hom.rot.x.x = c1 * t4 + s1 * t3;
00068 hom.rot.x.y = s1 * t4 - c1 * t3;
00069 hom.rot.x.z = -s23 * t1 - t5;
00070
00071
00072
00073 t1 = -c4 * c5 * s6 - s4 * c6;
00074 t2 = s23 * s5 * s6;
00075 t3 = c4 * c6 - s4 * c5 * s6;
00076 t4 = c23 * t1 + t2;
00077 t5 = c23 * s5 * s6;
00078
00079
00080 hom.rot.y.x = c1 * t4 + s1 * t3;
00081 hom.rot.y.y = s1 * t4 - c1 * t3;
00082 hom.rot.y.z = -s23 * t1 + t5;
00083
00084
00085
00086 t1 = c23 * c4 * s5 + s23 * c5;
00087
00088
00089 hom.rot.z.x = -c1 * t1 - s1 * s4 * s5;
00090 hom.rot.z.y = -s1 * t1 + c1 * s4 * s5;
00091 hom.rot.z.z = s23 * c4 * s5 - c23 * c5;
00092
00093
00094
00095 t1 = PUMA560_A2 * c2 + PUMA560_A3 * c23 - PUMA560_D4 * s23;
00096
00097
00098 hom.tran.x = c1 * t1 - PUMA560_D3 * s1;
00099 hom.tran.y = s1 * t1 + PUMA560_D3 * c1;
00100 hom.tran.z = -PUMA560_A3 * s23 - PUMA560_A2 * s2 - PUMA560_D4 * c23;
00101
00102
00103
00104 sumSq = hom.tran.x * hom.tran.x + hom.tran.y * hom.tran.y -
00105 PUMA560_D3 * PUMA560_D3;
00106 k = (sumSq + hom.tran.z * hom.tran.z - PUMA560_A2 * PUMA560_A2 -
00107 PUMA560_A3 * PUMA560_A3 - PUMA560_D4 * PUMA560_D4) /
00108 (2.0 * PUMA560_A2);
00109
00110
00111 *iflags = 0;
00112
00113
00114 if (fabs(joint[0] - atan2(hom.tran.y, hom.tran.x) +
00115 atan2(PUMA560_D3, -sqrt(sumSq))) < FLAG_FUZZ)
00116 {
00117 *iflags |= PUMA560_SHOULDER_RIGHT;
00118 }
00119
00120
00121 if (fabs(joint[2] - atan2(PUMA560_A3, PUMA560_D4) +
00122 atan2(k, -sqrt(PUMA560_A3 * PUMA560_A3 +
00123 PUMA560_D4 * PUMA560_D4 - k * k))) < FLAG_FUZZ)
00124 {
00125 *iflags |= PUMA560_ELBOW_DOWN;
00126 }
00127
00128
00129 t1 = -hom.rot.z.x * s1 + hom.rot.z.y * c1;
00130 t2 = -hom.rot.z.x * c1 * c23 - hom.rot.z.y * s1 * c23 +
00131 hom.rot.z.z * s23;
00132 if (fabs(t1) < SINGULAR_FUZZ && fabs(t2) < SINGULAR_FUZZ)
00133 {
00134 *iflags |= PUMA560_SINGULAR;
00135 }
00136
00137
00138 else{
00139 if (! (fabs(joint[3] - atan2(t1, t2)) < FLAG_FUZZ))
00140 {
00141 *iflags |= PUMA560_WRIST_FLIP;
00142 }
00143 }
00144
00145
00146 pmHomPoseConvert(hom, &worldPose);
00147 pmQuatRpyConvert(worldPose.rot,&rpy);
00148 world->tran = worldPose.tran;
00149 world->a = rpy.r*180.0/PM_PI;
00150 world->b = rpy.p*180.0/PM_PI;
00151 world->c = rpy.y*180.0/PM_PI;
00152
00153
00154
00155 return 0;
00156 }
00157
00158 int kinematicsInverse(const EmcPose * world,
00159 double * joint,
00160 const KINEMATICS_INVERSE_FLAGS * iflags,
00161 KINEMATICS_FORWARD_FLAGS * fflags)
00162 {
00163 PmHomogeneous hom;
00164 PmPose worldPose;
00165 PmRpy rpy;
00166 int singular;
00167
00168 double t1, t2, t3;
00169 double k;
00170 double sumSq;
00171
00172 double th1;
00173 double th3;
00174 double th23;
00175 double th2;
00176 double th4;
00177 double th5;
00178 double th6;
00179
00180 double s1, c1;
00181 double s3, c3;
00182 double s23, c23;
00183 double s4, c4;
00184 double s5, c5;
00185 double s6, c6;
00186
00187
00188 *fflags = 0;
00189
00190
00191 worldPose.tran = world->tran;
00192 rpy.r = world->a*PM_PI/180.0;
00193 rpy.p = world->b*PM_PI/180.0;
00194 rpy.y = world->c*PM_PI/180.0;
00195 pmRpyQuatConvert(rpy,&worldPose.rot);
00196 pmPoseHomConvert(worldPose, &hom);
00197
00198
00199
00200
00201 sumSq = hom.tran.x * hom.tran.x + hom.tran.y * hom.tran.y -
00202 PUMA560_D3 * PUMA560_D3;
00203
00204
00205 if (*iflags & PUMA560_SHOULDER_RIGHT){
00206 th1 = atan2(hom.tran.y, hom.tran.x) - atan2(PUMA560_D3, -sqrt(sumSq));
00207 }
00208 else{
00209 th1 = atan2(hom.tran.y, hom.tran.x) - atan2(PUMA560_D3, sqrt(sumSq));
00210 }
00211
00212
00213 s1 = sin(th1);
00214 c1 = cos(th1);
00215
00216
00217
00218 k = (sumSq + hom.tran.z * hom.tran.z - PUMA560_A2 * PUMA560_A2 -
00219 PUMA560_A3 * PUMA560_A3 - PUMA560_D4 * PUMA560_D4) / (2.0 * PUMA560_A2);
00220
00221
00222 if (*iflags & PUMA560_ELBOW_DOWN){
00223 th3 = atan2(PUMA560_A3, PUMA560_D4) - atan2(k, -sqrt(PUMA560_A3 * PUMA560_A3 + PUMA560_D4 * PUMA560_D4 - k * k));
00224 }
00225 else{
00226 th3 = atan2(PUMA560_A3, PUMA560_D4) -
00227 atan2(k, sqrt(PUMA560_A3 * PUMA560_A3 + PUMA560_D4 * PUMA560_D4 - k * k));
00228 }
00229
00230
00231 s3 = sin(th3);
00232 c3 = cos(th3);
00233
00234
00235
00236 t1 = (-PUMA560_A3 - PUMA560_A2 * c3) * hom.tran.z +
00237 (c1 * hom.tran.x + s1 * hom.tran.y) * (PUMA560_A2 * s3 - PUMA560_D4);
00238 t2 = (PUMA560_A2 * s3 - PUMA560_D4) * hom.tran.z +
00239 (PUMA560_A3 + PUMA560_A2 * c3) * (c1 * hom.tran.x + s1 * hom.tran.y);
00240 t3 = hom.tran.z * hom.tran.z + (c1 * hom.tran.x + s1 * hom.tran.y) *
00241 (c1 * hom.tran.x + s1 * hom.tran.y);
00242
00243 th23 = atan2(t1, t2);
00244 th2 = th23 - th3;
00245
00246
00247 s23 = t1 / t3;
00248 c23 = t2 / t3;
00249
00250
00251
00252 t1 = -hom.rot.z.x * s1 + hom.rot.z.y * c1;
00253 t2 = -hom.rot.z.x * c1 * c23 - hom.rot.z.y * s1 * c23 + hom.rot.z.z * s23;
00254 if (fabs(t1) < SINGULAR_FUZZ && fabs(t2) < SINGULAR_FUZZ){
00255 singular = 1;
00256 *fflags |= PUMA560_REACH;
00257 th4 = joint[3];
00258 }
00259 else{
00260 singular = 0;
00261 th4 = atan2(t1, t2);
00262 }
00263
00264
00265 s4 = sin(th4);
00266 c4 = cos(th4);
00267
00268
00269
00270 s5 = hom.rot.z.z * (s23 * c4) -
00271 hom.rot.z.x * (c1 * c23 * c4 + s1 * s4) -
00272 hom.rot.z.y * (s1 * c23 * c4 - c1 * s4);
00273 c5 =-hom.rot.z.x * (c1 * s23) - hom.rot.z.y *
00274 (s1 * s23) - hom.rot.z.z * c23;
00275 th5 = atan2(s5, c5);
00276
00277
00278
00279 s6 = hom.rot.x.z * (s23 * s4) - hom.rot.x.x *
00280 (c1 * c23 * s4 - s1 * c4) - hom.rot.x.y *
00281 (s1 * c23 * s4 + c1 * c4);
00282 c6 = hom.rot.x.x * ((c1 * c23 * c4 + s1 * s4) *
00283 c5 - c1 * s23 * s5) + hom.rot.x.y *
00284 ((s1 * c23 * c4 - c1 * s4) * c5 - s1 * s23 * s5) -
00285 hom.rot.x.z * (s23 * c4 * c5 + c23 * s5);
00286 th6 = atan2(s6, c6);
00287
00288
00289 if (*iflags & PUMA560_WRIST_FLIP){
00290 th4 = th4 + PM_PI;
00291 th5 = -th5;
00292 th6 = th6 + PM_PI;
00293 }
00294
00295
00296 joint[0] = th1;
00297 joint[1] = th2;
00298 joint[2] = th3;
00299 joint[3] = th4;
00300 joint[4] = th5;
00301 joint[5] = th6;
00302
00303 return singular == 0 ? 0 : -1;
00304 }
00305
00306 int kinematicsHome(EmcPose * world,
00307 double * joint,
00308 KINEMATICS_FORWARD_FLAGS * fflags,
00309 KINEMATICS_INVERSE_FLAGS * iflags)
00310 {
00311
00312 return kinematicsForward(joint, world, fflags, iflags);
00313 }
00314
00315 KINEMATICS_TYPE kinematicsType()
00316 {
00317 return KINEMATICS_SERIAL;
00318 }
00319
00320
00321
00322
00323
00324