#include <math.h>#include "posemath.h"#include "puma560kins.h"#include "emcmot.h"Include dependency graph for puma560kins.c:
Go to the source code of this file.
Functions | |
| int | kinematicsForward (const double *joint, EmcPose *world, const KINEMATICS_FORWARD_FLAGS *fflags, KINEMATICS_INVERSE_FLAGS *iflags) |
| int | kinematicsInverse (const EmcPose *world, double *joint, const KINEMATICS_INVERSE_FLAGS *iflags, KINEMATICS_FORWARD_FLAGS *fflags) |
| int | kinematicsHome (EmcPose *world, double *joint, KINEMATICS_FORWARD_FLAGS *fflags, KINEMATICS_INVERSE_FLAGS *iflags) |
| KINEMATICS_TYPE | kinematicsType () |
Variables | |
| double | PUMA560_A2 = 300.0 |
| double | PUMA560_A3 = 50.0 |
| double | PUMA560_D3 = 70.0 |
| double | PUMA560_D4 = 400.0 |
|
||||||||||||||||||||
|
Definition at line 23 of file puma560kins.c. Referenced by emcmotController(), and kinematicsHome().
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 /* Calculate sin of joints for future use */
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 /* Calculate cos of joints for future use */
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 /* Calculate terms to be used in definition of... */
00059 /* first column of rotation matrix. */
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 /* Define first column of rotation matrix */
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 /* Calculate terms to be used in definition of... */
00072 /* second column of rotation matrix. */
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 /* Define second column of rotation matrix */
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 /* Calculate term to be used in definition of... */
00085 /* third column of rotation matrix. */
00086 t1 = c23 * c4 * s5 + s23 * c5;
00087
00088 /* Define third column of rotation matrix */
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 /* Calculate term to be used in definition of... */
00094 /* position vector. */
00095 t1 = PUMA560_A2 * c2 + PUMA560_A3 * c23 - PUMA560_D4 * s23;
00096
00097 /* Define position vector */
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 /* Calculate terms to be used to... */
00103 /* determine flags. */
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 /* reset flags */
00111 *iflags = 0;
00112
00113 /* Set shoulder-up flag if necessary */
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 /* Set elbow down flag if necessary */
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 /* set singular flag if necessary */
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 /* if not singular set wrist flip flag if necessary */
00138 else{
00139 if (! (fabs(joint[3] - atan2(t1, t2)) < FLAG_FUZZ))
00140 {
00141 *iflags |= PUMA560_WRIST_FLIP;
00142 }
00143 }
00144
00145 /* convert hom.rot to world->quat */
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 /* return 0 and exit */
00155 return 0;
00156 }
|
|
||||||||||||||||||||
|
Definition at line 306 of file puma560kins.c. 00310 {
00311 /* use joints, set world */
00312 return kinematicsForward(joint, world, fflags, iflags);
00313 }
|
|
||||||||||||||||||||
|
Definition at line 158 of file puma560kins.c. 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 /* reset flags */
00188 *fflags = 0;
00189
00190 /* convert pose to hom */
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 /* Joint 1 (2 independent solutions) */
00199
00200 /* save sum of squares for this and subsequent calcs */
00201 sumSq = hom.tran.x * hom.tran.x + hom.tran.y * hom.tran.y -
00202 PUMA560_D3 * PUMA560_D3;
00203
00204 /* FIXME-- is use of + sqrt shoulder right or left? */
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 /* save sin, cos for later calcs */
00213 s1 = sin(th1);
00214 c1 = cos(th1);
00215
00216 /* Joint 3 (2 independent solutions) */
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 /* FIXME-- is use of + sqrt elbow up or down? */
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 /* compute sin, cos for later calcs */
00231 s3 = sin(th3);
00232 c3 = cos(th3);
00233
00234 /* Joint 2 */
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 /* compute sin, cos for later calcs */
00247 s23 = t1 / t3;
00248 c23 = t2 / t3;
00249
00250 /* Joint 4 */
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]; /* use current value */
00258 }
00259 else{
00260 singular = 0;
00261 th4 = atan2(t1, t2);
00262 }
00263
00264 /* compute sin, cos for later calcs */
00265 s4 = sin(th4);
00266 c4 = cos(th4);
00267
00268 /* Joint 5 */
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 /* Joint 6 */
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 /* FIXME-- is wrist flip the normal or offset results? */
00289 if (*iflags & PUMA560_WRIST_FLIP){
00290 th4 = th4 + PM_PI;
00291 th5 = -th5;
00292 th6 = th6 + PM_PI;
00293 }
00294
00295 /* copy out */
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 }
|
|
|
Definition at line 315 of file puma560kins.c. 00316 {
00317 return KINEMATICS_SERIAL;
00318 }
|
|
|
Definition at line 18 of file puma560kins.c. |
|
|
Definition at line 19 of file puma560kins.c. |
|
|
Definition at line 20 of file puma560kins.c. |
|
|
Definition at line 21 of file puma560kins.c. |
1.2.11.1 written by Dimitri van Heesch,
© 1997-2001