IK constraints Quat-Euler, Euler-Quat
Hello
I wonder on how to proceed on this...I have these two quaternions, representing start/goal for an IK chain.
I want it to rotate a selected joint so the effector gets as close to the goal as possible...considering its local constraints.
I wish (if possible) to calculate a rotation quaternion, describing the rotation from start to goal. Then convert
that rotation into Euler angles....then adjust these angles
so it rotates as much as it can about its axis...ex
limits: x(-5,5) y(10,40) z (-15,15)
euler: (-10,30,20)
it gets adjusted to (-5,30,15)...then converted back into
a rotation quaternion....and used to calculate a "practical"
goal...the closest position to the real goal it can reach.
Im not sure if this is going to work...or if there is a better way. (interested to hear about it)
and I could really use som conversion routines for the rotation
Quaternion->Euler
and
Euler->Quaternion
using XYZ order...and the coordinatesystem is as follows
x is right
y is up
z is towards viewer
any helpful answers, thoughts is most welcomed....
/T
inline Quaternion MakeQFromEulerAngles(float x, float y, float z)
{
Quaternion q;
double roll = DegreesToRadians(x);
double pitch = DegreesToRadians(y);
double yaw = DegreesToRadians(z);
double cyaw, cpitch, croll, syaw, spitch, sroll;
double cyawcpitch, syawspitch, cyawspitch, syawcpitch;
cyaw = cos(0.5f * yaw);
cpitch = cos(0.5f * pitch);
croll = cos(0.5f * roll);
syaw = sin(0.5f * yaw);
spitch = sin(0.5f * pitch);
sroll = sin(0.5f * roll);
cyawcpitch = cyaw*cpitch;
syawspitch = syaw*spitch;
cyawspitch = cyaw*spitch;
syawcpitch = syaw*cpitch;
q.n = (float) (cyawcpitch * croll + syawspitch * sroll);
q.v.x = (float) (cyawcpitch * sroll - syawspitch * croll);
q.v.y = (float) (cyawspitch * croll + syawcpitch * sroll);
q.v.z = (float) (syawcpitch * croll - cyawspitch * sroll);
return q;
}
inline Vector MakeEulerAnglesFromQ(Quaternion q)
{
double r11, r21, r31, r32, r33, r12, r13;
double q00, q11, q22, q33;
double tmp;
Vector u;
q00 = q.n * q.n;
q11 = q.v.x * q.v.x;
q22 = q.v.y * q.v.y;
q33 = q.v.z * q.v.z;
r11 = q00 + q11 - q22 - q33;
r21 = 2 * (q.v.x*q.v.y + q.n*q.v.z);
r31 = 2 * (q.v.x*q.v.z - q.n*q.v.y);
r32 = 2 * (q.v.y*q.v.z + q.n*q.v.x);
r33 = q00 - q11 - q22 + q33;
tmp = fabs(r31);
if(tmp > 0.999999)
{
r12 = 2 * (q.v.x*q.v.y - q.n*q.v.z);
r13 = 2 * (q.v.x*q.v.z + q.n*q.v.y);
u.x = RadiansToDegrees(0.0f); //roll
u.y = RadiansToDegrees((float) (-(pi/2) * r31/tmp)); // pitch
u.z = RadiansToDegrees((float) atan2(-r12,(-r31*r13))); // yaw
return u;
}
u.x = RadiansToDegrees((float) atan2(r32,r33)); // roll
u.y = RadiansToDegrees((float) asin(-r31)); // pitch
u.z = RadiansToDegrees((float) atan2(r21,r11)); // yaw
return u;
}
<code>
inline Quaternion MakeQFromEulerAngles(float x, float y, float z)
{
Quaternion q;
double roll = DegreesToRadians(x);
double pitch = DegreesToRadians(y);
double yaw = DegreesToRadians(z);
double cyaw, cpitch, croll, syaw, spitch, sroll;
double cyawcpitch, syawspitch, cyawspitch, syawcpitch;
cyaw = cos(0.5f * yaw);
cpitch = cos(0.5f * pitch);
croll = cos(0.5f * roll);
syaw = sin(0.5f * yaw);
spitch = sin(0.5f * pitch);
sroll = sin(0.5f * roll);
cyawcpitch = cyaw*cpitch;
syawspitch = syaw*spitch;
cyawspitch = cyaw*spitch;
syawcpitch = syaw*cpitch;
q.n = (float) (cyawcpitch * croll + syawspitch * sroll);
q.v.x = (float) (cyawcpitch * sroll - syawspitch * croll);
q.v.y = (float) (cyawspitch * croll + syawcpitch * sroll);
q.v.z = (float) (syawcpitch * croll - cyawspitch * sroll);
return q;
}
</code>
<src>
inline Vector MakeEulerAnglesFromQ(Quaternion q)
{
double r11, r21, r31, r32, r33, r12, r13;
double q00, q11, q22, q33;
double tmp;
Vector u;
q00 = q.n * q.n;
q11 = q.v.x * q.v.x;
q22 = q.v.y * q.v.y;
q33 = q.v.z * q.v.z;
r11 = q00 + q11 - q22 - q33;
r21 = 2 * (q.v.x*q.v.y + q.n*q.v.z);
r31 = 2 * (q.v.x*q.v.z - q.n*q.v.y);
r32 = 2 * (q.v.y*q.v.z + q.n*q.v.x);
r33 = q00 - q11 - q22 + q33;
tmp = fabs(r31);
if(tmp > 0.999999)
{
r12 = 2 * (q.v.x*q.v.y - q.n*q.v.z);
r13 = 2 * (q.v.x*q.v.z + q.n*q.v.y);
u.x = RadiansToDegrees(0.0f); //roll
u.y = RadiansToDegrees((float) (-(pi/2) * r31/tmp)); // pitch
u.z = RadiansToDegrees((float) atan2(-r12,(-r31*r13))); // yaw
return u;
}
u.x = RadiansToDegrees((float) atan2(r32,r33)); // roll
u.y = RadiansToDegrees((float) asin(-r31)); // pitch
u.z = RadiansToDegrees((float) atan2(r21,r11)); // yaw
return u;
}
</src>
Well not sure how to do the source post, but anywho this should get ya started, make sure your roll, pitch, and yaw are on the same coordinates... been awhile since I went over this code....
inline Quaternion MakeQFromEulerAngles(float x, float y, float z)
{
Quaternion q;
double roll = DegreesToRadians(x);
double pitch = DegreesToRadians(y);
double yaw = DegreesToRadians(z);
double cyaw, cpitch, croll, syaw, spitch, sroll;
double cyawcpitch, syawspitch, cyawspitch, syawcpitch;
cyaw = cos(0.5f * yaw);
cpitch = cos(0.5f * pitch);
croll = cos(0.5f * roll);
syaw = sin(0.5f * yaw);
spitch = sin(0.5f * pitch);
sroll = sin(0.5f * roll);
cyawcpitch = cyaw*cpitch;
syawspitch = syaw*spitch;
cyawspitch = cyaw*spitch;
syawcpitch = syaw*cpitch;
q.n = (float) (cyawcpitch * croll + syawspitch * sroll);
q.v.x = (float) (cyawcpitch * sroll - syawspitch * croll);
q.v.y = (float) (cyawspitch * croll + syawcpitch * sroll);
q.v.z = (float) (syawcpitch * croll - cyawspitch * sroll);
return q;
}
</code>
<src>
inline Vector MakeEulerAnglesFromQ(Quaternion q)
{
double r11, r21, r31, r32, r33, r12, r13;
double q00, q11, q22, q33;
double tmp;
Vector u;
q00 = q.n * q.n;
q11 = q.v.x * q.v.x;
q22 = q.v.y * q.v.y;
q33 = q.v.z * q.v.z;
r11 = q00 + q11 - q22 - q33;
r21 = 2 * (q.v.x*q.v.y + q.n*q.v.z);
r31 = 2 * (q.v.x*q.v.z - q.n*q.v.y);
r32 = 2 * (q.v.y*q.v.z + q.n*q.v.x);
r33 = q00 - q11 - q22 + q33;
tmp = fabs(r31);
if(tmp > 0.999999)
{
r12 = 2 * (q.v.x*q.v.y - q.n*q.v.z);
r13 = 2 * (q.v.x*q.v.z + q.n*q.v.y);
u.x = RadiansToDegrees(0.0f); //roll
u.y = RadiansToDegrees((float) (-(pi/2) * r31/tmp)); // pitch
u.z = RadiansToDegrees((float) atan2(-r12,(-r31*r13))); // yaw
return u;
}
u.x = RadiansToDegrees((float) atan2(r32,r33)); // roll
u.y = RadiansToDegrees((float) asin(-r31)); // pitch
u.z = RadiansToDegrees((float) atan2(r21,r11)); // yaw
return u;
}
</src>
Well not sure how to do the source post, but anywho this should get ya started, make sure your roll, pitch, and yaw are on the same coordinates... been awhile since I went over this code....
This topic is closed to new replies.
Advertisement
Popular Topics
Advertisement