On 5/15/2011 3:12 PM, Viesturs Lācis 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);
> }
>
Viesturs:
Whenever I get unphysically large results I suspect I'm trying to divide
by zero or close to it, but it's hard to say anything useful without
also looking at your conversion routines pmRpyZyzConvert and
pmZyzPpyConvert. Did you post them somewhere already? How about the
definitions of your data structures such as EmcPose?
Regards,
Kent
------------------------------------------------------------------------------
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
