On 5/21/2011 10:25 AM, Viesturs Lācis wrote:
> 2011/5/21 Kent A. Reed<knbr...@erols.com>:
>> True, true. But I note that the functions like pmQuatZyzConvert are
>> marked "FIXME - - need direct equations"". In mathematics, the two
>> approaches (one conversion or two concatenated conversions) are
>> equivalent. In computer math, not so much. It may make no practical
>> difference for Viesturs' application but I like the direct approach.
> Yohoooo!!!
> I have made some progress in genserkins with linear joints:
> The travel distance on linear joint matches in both joint and teleop mode!
>
> Now the only remaining problem is to allign XYZ of the arm with XYZ of
> the base. Now only Z matches.
> It _should_ be easy with alpha and theta angles of DH parameters, but
> it is not... At least for me. But I hope that I will fix it and then
> those zyz<-->rpy conversions will not be necessary.
>
> Viesturs
Viesturs:
If linear joints work for you, then so much the better.
Just in case you end up needing or just want to try the zyz<-->rpy
conversions, you can take the two-step approach that Andrew suggested.
In your forward and inverse routines, you will need to define a scratch
rotation matrix to hold an intermediate result and replace the single
calls to posemath routines with two, back-to-back calls. I took the
liberty of replacing the constants (180/PM_PI) and (PM_PI/180) with
their equivalent definitions in posemath.
---snip---
int kinematicsForward(const double * joint,
EmcPose * world,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags)
{
double a3, a4, a5;
PmEulerZyz zyz;
PmRpy rpy;
PmRotationMatrix rotMat;
/* convert joint angles to radians for sin() and cos() */
a3 = joint[3] * TO_RAD;
a4 = joint[4] * TO_RAD;
a5 = joint[5] * TO_RAD;
/* 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);
pmZyzMatConvert(zyz,&rotMat);
pmMatRpyConvert(rotMat,&rpy);
world->tran.x = joint[0];
world->tran.y = joint[1];
world->tran.z = joint[2];
world->a = rpy.r * TO_DEG;
world->b = rpy.p * TO_DEG;
world->c = rpy.y * TO_DEG;
return (0);
}
int kinematicsInverse(const EmcPose * world,
double * joint,
const KINEMATICS_INVERSE_FLAGS * iflags,
KINEMATICS_FORWARD_FLAGS * fflags)
{
PmEulerZyz zyz;
PmRpy rpy;
PmRotationMatrix rotMat;
rpy.r = world->a * TO_RAD;
rpy.p = world->b * TO_RAD;
rpy.y = world->c * TO_RAD;
/* convert Rpy angles to Euler's Zyz */
# pmRpyZyzConvert(rpy,&zyz);
pmRpyMatConvert(rpy,&rotMat);
pmMatZyzConvert(rotMat,&zyz);
joint[0] = world->tran.x;
joint[1] = world->tran.y;
joint[2] = world->tran.z;
joint[3] = zyz.z * TO_DEG;
joint[4] = zyz.y * TO_DEG;
joint[5] = zyz.zp * TO_DEG;
return (0);
}
---snip---
I did this quickly and didn't attempt to compile and link, so check my
work carefully, especially any use of pointers---they are always my
nemesis using C!
This solution has several appealing characteristics.
1) you can implement it right now, without any changes to posemath.
2) I can focus on the direct functions without concern that I'm standing
in the way.
Good luck with whichever approach you take...
Regards,
Kent
------------------------------------------------------------------------------
What Every C/C++ and Fortran developer Should Know!
Read this article and learn how Intel has extended the reach of its
next-generation tools to help Windows* and Linux* C/C++ and Fortran
developers boost performance applications - including clusters.
http://p.sf.net/sfu/intel-dev2devmay
_______________________________________________
Emc-users mailing list
Emc-users@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/emc-users
