Why dont you set up a quick testbed and some prinf's I am sure that is not going to take more than 20 lines of code.
j. On Sun, May 15, 2011 at 9:12 PM, Viesturs Lācis <[email protected]>wrote: > Hello! > > I am trying to develop kinematics module and I think that Euler's Zyz > angles would be very appropriate to represent joints' positions. > And for that I want to use Zyz to Rpy convertion in forward kins and > Rpy to Zyz convertion in inverse kins. > > The problem is that I cannot get that conversion working. Not even as > a pure concept - I have set the formulas as simple as possible just to > get it working, and then I would like to adjust them so that they > match the machine. > > What I get in EMC is: > 1) module compiles without errors, so I suspect thatI have got the > syntax correct. > 2) EMC starts up without errors; > 3) Homing of joints works ok (I am running a simulated machine without > any particular model loaded); > 4) switching to world mode and I get crazy values in A, B and C; > 5) sometimes I can quickly switch back to joint mode, where joint values > are ok; > 6) anyway, all the times immediately or after few seconds I get error > taht some of rotary joints have exceeded negative limit. And then > switching to joint mode shows joint value of something like > -6,175e+278 > > And that is it. > > > Question to the audience - has anyone ever used Euler angles in EMC2 > and functions that convert them to rpy, rotation matrix or whatever > else? > > > Thanks in advance! > > Viesturs > > > Here is the forward and inverse kinematics section from the module: > > int kinematicsForward(const double * joint, > EmcPose * world, > const KINEMATICS_FORWARD_FLAGS * fflags, > KINEMATICS_INVERSE_FLAGS * iflags) > { > double a3, a4, a5; > > PmEulerZyz zyz; > PmRpy rpy; > > /* convert joint angles to radians for sin() and cos() */ > > a3 = joint[3] * ( PM_PI / 180 ); > a4 = joint[4] * ( PM_PI / 180 ); > a5 = joint[5] * ( PM_PI / 180 ); > > /* assign joint values to Zyz angles */ > zyz.z = a3; > zyz.y = a4; > zyz.zp = a5; > > /* use Euler's Zyz to Rpy conversion*/ > pmZyzRpyConvert(zyz, &rpy); > > world->tran.x = joint[0]; > world->tran.y = joint[1]; > world->tran.z = joint[2]; > world->a = rpy.r * 180 / PM_PI; > world->b = rpy.p * 180 / PM_PI; > world->c = rpy.y * 180 / PM_PI; > > return (0); > } > > int kinematicsInverse(const EmcPose * world, > double * joint, > const KINEMATICS_INVERSE_FLAGS * iflags, > KINEMATICS_FORWARD_FLAGS * fflags) > { > PmEulerZyz zyz; > PmRpy rpy; > > rpy.r = world->a * ( PM_PI / 180 ); > rpy.p = world->b * ( PM_PI / 180 ); > rpy.y = world->c * ( PM_PI / 180 ); > > /* convert Rpy angles to Euler's Zyz */ > pmRpyZyzConvert(rpy, &zyz); > > joint[0] = world->tran.x; > joint[1] = world->tran.y; > joint[2] = world->tran.z; > joint[3] = zyz.z * (180 / PM_PI); > joint[4] = zyz.y * (180 / PM_PI); > joint[5] = zyz.zp * (180 / PM_PI); > > return (0); > } > > > ------------------------------------------------------------------------------ > Achieve unprecedented app performance and reliability > What every C/C++ and Fortran developer should know. > Learn how Intel has extended the reach of its next-generation tools > to help boost performance applications - inlcuding clusters. > http://p.sf.net/sfu/intel-dev2devmay > _______________________________________________ > Emc-users mailing list > [email protected] > https://lists.sourceforge.net/lists/listinfo/emc-users > ------------------------------------------------------------------------------ Achieve unprecedented app performance and reliability What every C/C++ and Fortran developer should know. Learn how Intel has extended the reach of its next-generation tools to help boost performance applications - inlcuding clusters. http://p.sf.net/sfu/intel-dev2devmay _______________________________________________ Emc-users mailing list [email protected] https://lists.sourceforge.net/lists/listinfo/emc-users
