# IK constraints Quat-Euler, Euler-Quat

This topic is 5132 days old which is more than the 365 day threshold we allow for new replies. Please post a new topic.

## Recommended Posts

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

##### Share on other sites

inline Quaternion MakeQFromEulerAngles(float x, float y, float z)
{
Quaternion q;

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.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;

}

##### Share on other sites
<code>
inline Quaternion MakeQFromEulerAngles(float x, float y, float z)
{
Quaternion q;

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.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....

##### Share on other sites
thanks a lot!!!.... :)

1. 1
2. 2
3. 3
4. 4
Rutin
13
5. 5

• 12
• 15
• 9
• 14
• 10
• ### Forum Statistics

• Total Topics
632656
• Total Posts
3007678
• ### Who's Online (See full list)

There are no registered users currently online

×