I think that I give up trying to do this by myself (see below on reasons, that have driven me to the desperation) and so I would like to ask, if anyone can write a kinematics module for me and what would be the fee for that?
2010/6/9 Andy Pugh <[email protected]>: > > Indeed, look in genserkins for an example of that sort of calculation, > from fixed geometrical constants. For example rotatekins.c does all the maths > in > lines 24,25,43 and 45 to make a normal XYZ machine act as if it has a > rotary table. Your module would end up looking a lot like that. > Where can I find more detailed information, what does each kinematics model do, so that I can try to figure out, what parts of which kinematics module might be useful for me? Unfortunately description "Kinematics for a generalised serial kinematics machine" in genserkins.c does not tell me anything, What is serial kinematics? Also I do not understand, what exactly is the purpose of rotatekins. Does it accept G-code for XYZC and transforms it into XYZ without C movements? > > Indeed, look in genserkins for an example of that sort of calculation, from > fixed > geometrical constants. > I took a closer look at genserkins.c file. Here arethe sections, which determine forward and inverse kinematics: int kinematicsForward(const double *joint, EmcPose * world, const KINEMATICS_FORWARD_FLAGS * fflags, KINEMATICS_INVERSE_FLAGS * iflags) { go_pose *pos; go_rpy rpy; go_real jcopy[GENSER_MAX_JOINTS]; // will hold the radian conversion of joints int ret = 0; int i, changed=0; for (i=0; i< 6; i++) { // FIXME - debug hack if (!GO_ROT_CLOSE(j[i],joint[i])) changed = 1; // convert to radians to pass to genser_kin_fwd jcopy[i] = joint[i] * PM_PI / 180; } if (changed) { for (i=0; i< 6; i++) j[i] = joint[i]; // rtapi_print("kinematicsForward(joints: %f %f %f %f %f %f)\n", joint[0],joint[1],joint[2],joint[3],joint[4],joint[5]); } // AJ: convert from emc2 coords (XYZABC - which are actually rpy euler // angles) // to go angles (quaternions) pos = haldata->pos; rpy.y = world->c * PM_PI / 180; rpy.p = world->b * PM_PI / 180; rpy.r = world->a * PM_PI / 180; go_rpy_quat_convert(&rpy, &pos->rot); pos->tran.x = world->tran.x; pos->tran.y = world->tran.y; pos->tran.z = world->tran.z; // pos will be the world location // jcopy: joitn position in radians ret = genser_kin_fwd(KINS_PTR, jcopy, pos); if (ret < 0) return ret; // AJ: convert back to emc2 coords ret = go_quat_rpy_convert(&pos->rot, &rpy); if (ret < 0) return ret; world->tran.x = pos->tran.x; world->tran.y = pos->tran.y; world->tran.z = pos->tran.z; world->a = rpy.r * 180 / PM_PI; world->b = rpy.p * 180 / PM_PI; world->c = rpy.y * 180 / PM_PI; if (changed) { // rtapi_print("kinematicsForward(world: %f %f %f %f %f %f)\n", world->tran.x, world->tran.y, world->tran.z, world->a, world->b, world->c); } return 0; } int genser_kin_fwd(void *kins, const go_real * joints, go_pose * pos) { genser_struct *genser = kins; go_link linkout[GENSER_MAX_JOINTS]; int link; int retval; genser_kin_init(); for (link = 0; link < genser->link_num; link++) { retval = go_link_joint_set(&genser->links[link], joints[link], &linkout[link]); if (GO_RESULT_OK != retval) return retval; } retval = go_link_pose_build(linkout, genser->link_num, pos); if (GO_RESULT_OK != retval) return retval; return GO_RESULT_OK; } int kinematicsInverse(const EmcPose * world, double *joints, const KINEMATICS_INVERSE_FLAGS * iflags, KINEMATICS_FORWARD_FLAGS * fflags) { genser_struct *genser = KINS_PTR; GO_MATRIX_DECLARE(Jfwd, Jfwd_stg, 6, GENSER_MAX_JOINTS); GO_MATRIX_DECLARE(Jinv, Jinv_stg, GENSER_MAX_JOINTS, 6); go_pose T_L_0; go_real dvw[6]; go_real jest[GENSER_MAX_JOINTS]; go_real dj[GENSER_MAX_JOINTS]; go_pose pest, pestinv, Tdelta; // pos = converted pose from EmcPose go_rpy rpy; go_rvec rvec; go_cart cart; go_link linkout[GENSER_MAX_JOINTS]; int link; int smalls; int retval; // rtapi_print("kineInverse(joints: %f %f %f %f %f %f)\n", joints[0],joints[1],joints[2],joints[3],joints[4],joints[5]); // rtapi_print("kineInverse(world: %f %f %f %f %f %f)\n", world->tran.x, world->tran.y, world->tran.z, world->a, world->b, world->c); // genser_kin_init(); // FIXME-AJ: rpy or zyx ? rpy.y = world->c * PM_PI / 180; rpy.p = world->b * PM_PI / 180; rpy.r = world->a * PM_PI / 180; go_rpy_quat_convert(&rpy, &haldata->pos->rot); haldata->pos->tran.x = world->tran.x; haldata->pos->tran.y = world->tran.y; haldata->pos->tran.z = world->tran.z; go_matrix_init(Jfwd, Jfwd_stg, 6, genser->link_num); go_matrix_init(Jinv, Jinv_stg, genser->link_num, 6); /* jest[] is a copy of joints[], which is the joint estimate */ for (link = 0; link < genser->link_num; link++) { // jest, and the rest of joint related calcs are in radians jest[link] = joints[link] * (PM_PI / 180); } for (genser->iterations = 0; genser->iterations < genser->max_iterations; genser->iterations++) { /* update the Jacobians */ for (link = 0; link < genser->link_num; link++) { go_link_joint_set(&genser->links[link], jest[link], &linkout[link]); } retval = compute_jfwd(linkout, genser->link_num, &Jfwd, &T_L_0); if (GO_RESULT_OK != retval) { rtapi_print("ERR kI - compute_jfwd (joints: %f %f %f %f %f %f), (iterations=%d)\n", joints[0],joints[1],joints[2],joints[3],joints[4],joints[5], genser->iterations); return retval; } retval = compute_jinv(&Jfwd, &Jinv); if (GO_RESULT_OK != retval) { rtapi_print("ERR kI - compute_jinv (joints: %f %f %f %f %f %f), (iterations=%d)\n", joints[0],joints[1],joints[2],joints[3],joints[4],joints[5], genser->iterations); return retval; } /* pest is the resulting pose estimate given joint estimate */ genser_kin_fwd(KINS_PTR, jest, &pest); // printf("jest: %f %f %f %f %f %f\n",jest[0],jest[1],jest[2],jest[3],jest[4],jest[5]); /* pestinv is its inverse */ go_pose_inv(&pest, &pestinv); /* Tdelta is the incremental pose from pest to pos, such that 0 L 0 . pest * Tdelta = pos, or L L L L L 0 .Tdelta = pestinv * pos L 0 L */ go_pose_pose_mult(&pestinv, haldata->pos, &Tdelta); /* We need Tdelta in 0 frame, not pest frame, so rotate it back. Since it's effectively a velocity, we just rotate it, and don't translate it. */ /* first rotate the translation differential */ go_quat_cart_mult(&pest.rot, &Tdelta.tran, &cart); dvw[0] = cart.x; dvw[1] = cart.y; dvw[2] = cart.z; /* to rotate the rotation differential, convert it to a velocity screw and rotate that */ go_quat_rvec_convert(&Tdelta.rot, &rvec); cart.x = rvec.x; cart.y = rvec.y; cart.z = rvec.z; go_quat_cart_mult(&pest.rot, &cart, &cart); dvw[3] = cart.x; dvw[4] = cart.y; dvw[5] = cart.z; /* push the Cartesian velocity vector through the inverse Jacobian */ go_matrix_vector_mult(&Jinv, dvw, dj); /* check for small joint increments, if so we're done */ for (link = 0, smalls = 0; link < genser->link_num; link++) { if (GO_QUANTITY_LENGTH == linkout[link].quantity) { if (GO_TRAN_SMALL(dj[link])) smalls++; } else { if (GO_ROT_SMALL(dj[link])) smalls++; } } if (smalls == genser->link_num) { /* converged, copy jest[] out */ for (link = 0; link < genser->link_num; link++) { // convert from radians back to angles joints[link] = jest[link] * 180 / PM_PI; } // rtapi_print("DONEkineInverse(joints: %f %f %f %f %f %f), (iterations=%d)\n", joints[0],joints[1],joints[2],joints[3],joints[4],joints[5], genser->iterations); return GO_RESULT_OK; } /* else keep iterating */ for (link = 0; link < genser->link_num; link++) { jest[link] += dj[link]; //still in radians } } /* for (iterations) */ rtapi_print("ERRkineInverse(joints: %f %f %f %f %f %f), (iterations=%d)\n", joints[0],joints[1],joints[2],joints[3],joints[4],joints[5], genser->iterations); return GO_RESULT_ERROR; } What I do not understand ( actually I do not understand almost everything :] ), is where did these come from: rpy.y = world->c * PM_PI / 180; rpy.p = world->b * PM_PI / 180; rpy.r = world->a * PM_PI / 180; go_rpy_quat_convert(&rpy, &pos->rot); pos->tran.x = world->tran.x; pos->tran.y = world->tran.y; pos->tran.z = world->tran.z; world->tran.x = pos->tran.x; world->tran.y = pos->tran.y; world->tran.z = pos->tran.z; world->a = rpy.r * 180 / PM_PI; world->b = rpy.p * 180 / PM_PI; world->c = rpy.y * 180 / PM_PI; vel->v.x = vw[0]; vel->v.y = vw[1]; vel->v.z = vw[2]; vel->w.x = vw[3]; vel->w.y = vw[4]; vel->w.z = vw[5]; >From examples in manuals and also in rotatekins.c I understood, how kinematics module links joint coordinates with Carthesian coordinates with some kind of trigonometric or any other calculations and formulas, that allows acquiring correct conversion from one to another and vice versa. What I do not understand, is where did such things as: rpy.y world vel v.x vw[0] come from and what do they mean. I believe that last one has something to do with some joint parameter (position or velocity). Actually, I went through all 18 lessons in http://www.cprogramming.com/tutorial/c/lesson1.html and I still do not understand, what does these lines in the beginning of inverse kinematics section mean: GO_MATRIX_DECLARE(Jfwd, Jfwd_stg, 6, GENSER_MAX_JOINTS); GO_MATRIX_DECLARE(Jinv, Jinv_stg, GENSER_MAX_JOINTS, 6); go_pose T_L_0; go_real dvw[6]; go_real jest[GENSER_MAX_JOINTS]; go_real dj[GENSER_MAX_JOINTS]; go_pose pest, pestinv, Tdelta; // pos = converted pose from EmcPose go_rpy rpy; go_rvec rvec; go_cart cart; They contain large part of those unknown functions/variables or whatever they are, but I have no f***ing idea, where to find, what they mean. Viesturs ------------------------------------------------------------------------------ ThinkGeek and WIRED's GeekDad team up for the Ultimate GeekDad Father's Day Giveaway. ONE MASSIVE PRIZE to the lucky parental unit. See the prize list and enter to win: http://p.sf.net/sfu/thinkgeek-promo _______________________________________________ Emc-users mailing list [email protected] https://lists.sourceforge.net/lists/listinfo/emc-users
