Bugs item #2870683, was opened at 2009-09-30 11:56 Message generated for change (Tracker Item Submitted) made by nobody You can respond by visiting: https://sourceforge.net/tracker/?func=detail&atid=106744&aid=2870683&group_id=6744
Please note that this message will contain a full copy of the comment thread, including the initial issue submission, for this request, not just the latest update. Category: Motion Controller Group: None Status: Open Resolution: None Priority: 5 Private: No Submitted By: Nobody/Anonymous (nobody) Assigned to: John Kasunich (jmkasunich) Summary: rotatekins wrong inverse-forward transformations Initial Comment: I found possible error in src/emc/kinematics/rotatekins.c Inverse kinematics does not map forward kinematics.. git latest commit: commit 4615d273932bc32c177158fb3698a71bb08a57bb Author: Ben Lipkowitz <[email protected]> Date: Sat Dec 15 17:34:51 2007 +0000 allow arbitrary rotational offset - this might as well be useful as well as informative introduced an error by omiting sign in forward calculation as shown in this partial diff: - double c_rad = joints[5]*M_PI/180; - pos->tran.x = joints[0] * cos(c_rad) - joints[1] * sin(c_rad); - pos->tran.y = joints[0] * sin(c_rad) + joints[1] * cos(c_rad); + pos->tran.x = joints[0] * cos(-M_PI_4l) - joints[1] * sin(-M_PI_4l); + pos->tran.y = joints[0] * sin(-M_PI_4l) + joints[1] * cos(-M_PI_4l); pos->tran.z = joints[2]; pos->a = joints[3]; pos->b = joints[4]; @@ -39,9 +42,8 @@ int kinematicsInverse(const EmcPose * pos, const KINEMATICS_INVERSE_FLAGS * iflags, KINEMATICS_FORWARD_FLAGS * fflags) { - double c_rad = pos->c*M_PI/180; - joints[0] = pos->tran.x * cos(c_rad) - pos->tran.y * sin(c_rad); - joints[1] = pos->tran.x * sin(c_rad) + pos->tran.y * cos(c_rad); + joints[0] = pos->tran.x * cos(M_PI_4l) - pos->tran.y * sin(M_PI_4l); + joints[1] = pos->tran.x * sin(M_PI_4l) + pos->tran.y * cos(M_PI_4l); Forward kinematics used in old code -M_PI_4l and +M_PI_4l for inverse kinematics. This is ok but patch uses +c_rad value for inverse and forward calculation. In forward kinematics "-c_rad" value must be used (or calculation of c_rad must be changed by "minus" sign). I propose patch bellow to eliminate error in this kinematics code: --- a/src/emc/kinematics/rotatekins.c +++ b/src/emc/kinematics/rotatekins.c @@ -20,7 +20,7 @@ int kinematicsForward(const double *joints, const KINEMATICS_FORWARD_FLAGS * fflags, KINEMATICS_INVERSE_FLAGS * iflags) { - double c_rad = joints[5]*M_PI/180; + double c_rad = -joints[5]*M_PI/180; pos->tran.x = joints[0] * cos(c_rad) - joints[1] * sin(c_rad); pos->tran.y = joints[0] * sin(c_rad) + joints[1] * cos(c_rad); pos->tran.z = joints[2]; ---------------------------------------------------------------------- You can respond by visiting: https://sourceforge.net/tracker/?func=detail&atid=106744&aid=2870683&group_id=6744 ------------------------------------------------------------------------------ Come build with us! The BlackBerry® Developer Conference in SF, CA is the only developer event you need to attend this year. Jumpstart your developing skills, take BlackBerry mobile applications to market and stay ahead of the curve. Join us from November 9-12, 2009. Register now! http://p.sf.net/sfu/devconf _______________________________________________ Emc-developers mailing list [email protected] https://lists.sourceforge.net/lists/listinfo/emc-developers
