rendered paste body/* RRRRRRkins.c adapted from puma560.c Kinematics for RRRRRR Typ manipulator alpha a d range Basisparameter G1(Joint0) -90 10 0 -160---+160 G2(joint1) 0 455 0 -225---+45 G3(joint2) 90 10 0 -45 ---+225 G4(joint3) -90 0 440 -110---+170 (theoretisch >360) G5(joint4) 90 0 0 -110---+110 G6(joint5) 0 0 125 -266---+266 (theoretisch >360) Die alphawerte sind Basis der Formeln und als Homeparameter beim Wechsel-World vorgegeben Modification history:AJ: The alpha values are premises of the formulas, and should be home parameters when changing to world!? 13-Aug-1999 FMP gave functions new names, from emcmot.h 02-Feb-2007 Anpassung auf EMC2 und RRRRRR beginn*/#include "motion.h"#include "rtapi_math.h"#include "posemath.h"#include "puma560kins.h"#include "kinematics.h" /* decls for kinematicsForward, etc. *//* Basisparameter fuer Achslaengen */double RRRRRR_A1 = 10.0; /* Versatz Basis Drehpunkt */ /* Distance to point of rotation*/double RRRRRR_A2 = 455.0; /* arm1 laenge */ /*length of arm*/double RRRRRR_A3 = 0.0;double RRRRRR_D1 = 25.0; /* höhe Sockel */ /*height of platform/base*/double RRRRRR_D3 = 0.0; /* versatz joint 2-3 */ /*distance between joints 2 & 3 */double RRRRRR_D4 = 440.0; /* arm2 laenge */ /*arm lenght 2*/double RRRRRR_D6 = 120.0; /* laenge joint5-- TCP */ /*distance from joint 5 to TCP (tool center Point) */int kinematicsForward(const double * joint, EmcPose * world, const KINEMATICS_FORWARD_FLAGS * fflags, KINEMATICS_INVERSE_FLAGS * iflags){ double s1, s2, s3, s4, s5, s6; double c1, c2, c3, c4, c5, c6; double s23; double c23; double c46; double c56; double t1, t2, t3, t4, t5, t6, t7, t8; double sumSq, k; PmHomogeneous hom; PmPose worldPose; PmRpy rpy; /* Calculate sin of joints for future use */ s1 = sin(joint[0]); s2 = sin(joint[1]); s3 = sin(joint[2]); s4 = sin(joint[3]); s5 = sin(joint[4]); s6 = sin(joint[5]); /* Calculate cos of joints for future use */ c1 = cos(joint[0]); c2 = cos(joint[1]); c3 = cos(joint[2]); c4 = cos(joint[3]); c5 = cos(joint[4]); c6 = cos(joint[5]); s23 = s2 * s3; c23 = c2 * c3; c46 = c4 * c6; c56 = c5 * s6 * (-1); /* Calculate Therms to be used in definition of...*/ /* First column of rotation matrix*/ t1 = s4 * c5 * c6 + c4 * s6; t2 = c4 * c5 * c6 - s4 * s6; t3 = c2 * s3 + s2 * c3; t4 = c23 - s23; t5 = c4 * s5 * RRRRRR_D6; /* Define first column of rotation matrix */ hom.rot.x.x = c1 * t4 * t2 + s1 * t1 - s6 * c1 *t3; hom.rot.x.y = s1 * t4 * t2 - c1 * t1 - s1 * t3 * s6; hom.rot.x.z = t3 * t2 - s6 * s23 - c23; /* Calculate Therms tobe used in definition of...*/ /* second column of rotation matrix*/ t6 = c4 * (c5 * -s6) - (s4 * c6); /* Define second column of rotation matrix */ hom.rot.y.x = c1 * t4 * t6 + s1 * s4 * c56 + c46 - c1 * t3 * c6; hom.rot.y.y = s1 * t4 * t6 - c1 * s4 * c56 + c46 - s1 * t3 * c6; hom.rot.y.z = t3 * t2 - s23 - c23 * c6; /* Calculate Therms tobe used in definition of...*/ /* third column of rotation matrix*/ t7 = s4 * s5 * RRRRRR_D6; t8 = RRRRRR_A2 * c2; /* Define third column of rotation matrix */ hom.rot.z.x = c1 * t4 * c4 * s5 + s1 * s4 * s5; hom.rot.z.y = s1 * t4 * c4 * s5 - c1 * s4 * s5; hom.rot.z.z = t3 * c4 * s5; /* define position vektor */ hom.tran.x = c1 * t4 * t5 + s1 * t7 + c1 * t3 * RRRRRR_D4 + c1 * t8 + s1 + RRRRRR_A1 * c1; hom.tran.y = s1 * t4 * t5 -c1 * t7 + s1 * t3 * RRRRRR_D4 + s1 * t8 - c1 + RRRRRR_A1 * s1; hom.tran.z = t3 * t5 + s23 - c23 * RRRRRR_D4 + RRRRRR_A2 * s2 + RRRRRR_D1; /* calculate terms to be used to... */ /* determine flags. */ sumSq = hom.tran.x * hom.tran.x + hom.tran.y * hom.tran.y - RRRRRR_D3 * RRRRRR_D3; k = (sumSq + hom.tran.z * hom.tran.z - RRRRRR_A2 * RRRRRR_A2 - RRRRRR_D4 * RRRRRR_D4) / (2.0 * RRRRRR_A2); /* reset flags */ *iflags = 0; /* Set shoulder-up-flag if necessary */ if (fabs(joint[0] - atan2(hom.tran.y, hom.tran.x) + atan2(RRRRRR_D3, -sqrt(sumSq))) < FLAG_FUZZ) { *iflags |= PUMA560_SHOULDER_RIGHT; } /* Set elbow-down-flag if necessary */ if (fabs(joint[2] - atan2(RRRRRR_A3, RRRRRR_D4) + atan2(k, -sqrt(RRRRRR_A3 * RRRRRR_A3 + RRRRRR_D4 + RRRRRR_D4 - k * k))) < FLAG_FUZZ) { *iflags |= PUMA560_ELBOW_DOWN; } /* Set singular-flag if necessary */ t1 = -hom.rot.z.x * s1 + hom.rot.z.y * c1; t2 = -hom.rot.z.x * c1 * c23 - hom.rot.z.y * s1 * c23 + hom.rot.z.z * s23; if (fabs(t1) < SINGULAR_FUZZ && fabs(t2) < SINGULAR_FUZZ) { *iflags |= PUMA560_SINGULAR; } /* if not singular set wrist flip flag if necessary */ else{ if (! (fabs(joint[3] - atan2(t1, t2)) < FLAG_FUZZ)) { *iflags |= PUMA560_WRIST_FLIP; } } /* convert hom.rot to world->quat */ pmHomPoseConvert(hom, &worldPose); pmQuatRpyConvert(worldPose.rot,&rpy); world->tran = worldPose.tran; world->a = rpy.r*180.0/PM_PI; world->b = rpy.p*180.0/PM_PI; world->c = rpy.y*180.0/PM_PI; /* return 0 and exit */ return 0;}int kinematicsInverse(const EmcPose * world, double * joint, const KINEMATICS_INVERSE_FLAGS * iflags, KINEMATICS_FORWARD_FLAGS * fflags){ PmHomogeneous hom; PmPose worldPose; PmRpy rpy; int singular; double t1, t2, t3; double k; double sumSq; double th1; double th3; double th23; double th2; double th4; double th5; double th6; double dtxy, dtz, ca, sa, cb, sb, cg, sg; double s1, c1; double s3, c3; double s23, c23; double s4, c4; double s5, c5; double s6, c6; /* reset flags */ *fflags = 0; /* convert pose to hom */ worldPose.tran = world->tran; rpy.r = world->a*PM_PI/180.0; rpy.p = world->b*PM_PI/180.0; rpy.y = world->c*PM_PI/180.0; pmRpyQuatConvert(rpy,&worldPose.rot); pmPoseHomConvert(worldPose, &hom); /* Joint 1 ( zwei unabh.Lösungen ) */ /* save sum of squares for this and subsequent calcs */ sumSq = hom.tran.x * hom.tran.x + hom.tran.y * hom.tran.y - RRRRRR_D3 * RRRRRR_D3; if (*iflags & PUMA560_SHOULDER_RIGHT){ th1 = atan2(hom.tran.y, hom.tran.x) - atan2(RRRRRR_D3, -sqrt (sumSq)); } else { th1 = atan2(hom.tran.y, hom.tran.x) - atan2(RRRRRR_D3, sqrt(sumSq)); } /* Save Sin for later calcs */ s1 = sin(th1); c1 = cos(th1); /* Joint 2 (Eigenversuch) */ dtxy = sqrt (hom.tran.x * hom.tran.x + hom.tran.y * hom.tran.y ) - RRRRRR_A1; dtz = hom.tran.z - RRRRRR_D1; t1 = sqrt ( dtz * dtz + dtxy * dtxy ); ca = dtxy / t1; sa = dtz / t1; cb = (RRRRRR_A2 * RRRRRR_A2 + t1 * t1 - RRRRRR_D4 * RRRRRR_D4) / (2.0 * RRRRRR_A2 * t1); sb = sqrt ( 1.0 - cb * cb); th2 = atan2(sb,cb) + atan2(dtz,dtxy); /* Joint 3 ( eigenversuch) */ cg = (RRRRRR_A2 * RRRRRR_A2 + RRRRRR_D4 * RRRRRR_D4 - t1 * t1) / (2.0 * RRRRRR_A2 * RRRRRR_D4); sg = sqrt ( 1- cg * cg); th3 = atan2(sg,cg) - PM_PI/2.0; /* copy out */ joint[0] = th1; joint[1] = th2; joint[2] = th3; joint[3] = world->a; joint[4] = world->b; joint[5] = world->c; return singular == 0 ? 0 : -1;}int kinematicsHome(EmcPose * world, double * joint, KINEMATICS_FORWARD_FLAGS * fflags, KINEMATICS_INVERSE_FLAGS * iflags){ /* use joints, set world */ return kinematicsForward(joint, world, fflags, iflags);}KINEMATICS_TYPE kinematicsType(){ return KINEMATICS_BOTH;}#ifdef RTAPI#include "rtapi.h" /* RTAPI realtime OS API */#include "rtapi_app.h" /* RTAPI realtime module decls */#include "hal.h"EXPORT_SYMBOL(kinematicsType);EXPORT_SYMBOL(kinematicsForward);EXPORT_SYMBOL(kinematicsInverse);int comp_id;int rtapi_app_main(void) { comp_id = hal_init("puma560kins"); if(comp_id > 0) { hal_ready(comp_id); return 0; } return comp_id;}void rtapi_app_exit(void) { hal_exit(comp_id); }#endif