Gimbal Angle and Rotation Matrix

[jenny_wui]

Could anyone tell me how can I derive rotation matrix from gimbal angles? Thaks every body in advance.

[pearm]

Rotation matrices can be found on wikipedia, http://en.wikipedia.org/wiki/Rotation_matrix

Edit: missed gimbal angle, since I don't really know what that is, this answer might not be what you're looking for at all.

[rogxx]

Maybe this can help You:



/***** try Euler RPY to rotation matrix: eul2mat.c *****/
// gcc -Wall -o eul2mat eul2mat.c -lm

// OpenGl standard: Column direction cosines:
//
// axs X axs Y axs Z OriginXYZ
// --------------------------------------
// | cosX cosX cosX X |
// M = | cosY cosY cosY Y |
// | cosZ cosZ cosZ Z |
// | |
// | 0 0 0 1 |
// --------------------------------------
// declare your matrix this way:
// double matrix[16];

#include <stdio.h>
#include <math.h>
#define TORAD 0.017453292519943295474371680597869271878153085708 6181640625

void MatrXMatr33(double *mat_1, double *mat_2, double *mat_r)
{
mat_r[0] = mat_1[0]*mat_2[0] + mat_1[1]*mat_2[4] + mat_1[2] *mat_2[8] ;
mat_r[1] = mat_1[0]*mat_2[1] + mat_1[1]*mat_2[5] + mat_1[2] *mat_2[9] ;
mat_r[2] = mat_1[0]*mat_2[2] + mat_1[1]*mat_2[6] + mat_1[2] *mat_2[10];

mat_r[4] = mat_1[4]*mat_2[0] + mat_1[5]*mat_2[4] + mat_1[6] *mat_2[8] ;
mat_r[5] = mat_1[4]*mat_2[1] + mat_1[5]*mat_2[5] + mat_1[6] *mat_2[9] ;
mat_r[6] = mat_1[4]*mat_2[2] + mat_1[5]*mat_2[6] + mat_1[6] *mat_2[10];

mat_r[8] = mat_1[8]*mat_2[0] + mat_1[9]*mat_2[4] + mat_1[10]*mat_2[8] ;
mat_r[9] = mat_1[8]*mat_2[1] + mat_1[9]*mat_2[5] + mat_1[10]*mat_2[9] ;
mat_r[10]= mat_1[8]*mat_2[2] + mat_1[9]*mat_2[6] + mat_1[10]*mat_2[10];
}// *** Fine MatrXMatr33() ***

void trasp_mat33(double *mat_1, double *mat_r)
{
mat_r[0]=mat_1[0]; mat_r[4]=mat_1[1]; mat_r[8] =mat_1[2];
mat_r[1]=mat_1[4]; mat_r[5]=mat_1[5]; mat_r[9] =mat_1[6];
mat_r[2]=mat_1[8]; mat_r[6]=mat_1[9]; mat_r[10]=mat_1[10];
}// *** Fine trasp_mat33() ***

void EulToDcm33(double *eulRPY, double *mat_ret1)
{
// eulRPY[2] = A -> (Z)
// eulRPY[1] = B -> (Y)
// eulRPY[0] = C -> (X)

double mat[16], mat_axs1[16], mat_ret2[16];
double rdAng=0.0;

mat_axs1[0]= 1.0 ; mat_axs1[4]= 0.0 ; mat_axs1[8] = 0.0 ; mat_axs1[12]= 0.0 ;
mat_axs1[1]= 0.0 ; mat_axs1[5]= 1.0 ; mat_axs1[9] = 0.0 ; mat_axs1[13]= 0.0 ;
mat_axs1[2]= 0.0 ; mat_axs1[6]= 0.0 ; mat_axs1[10]= 1.0 ; mat_axs1[14]= 0.0 ;
mat_axs1[3]= 0.0 ; mat_axs1[7]= 0.0 ; mat_axs1[11]= 0.0 ; mat_axs1[15]= 1.0 ;

rdAng = (-eulRPY[2]) * TORAD;
mat[0]= cos(rdAng); mat[4]=-sin(rdAng); mat[8] = 0.0 ;
mat[1]= sin(rdAng); mat[5]= cos(rdAng); mat[9] = 0.0 ;
mat[2]= 0.0 ; mat[6]= 0.0 ; mat[10]= 1.0 ;

MatrXMatr33(mat_axs1, mat, mat_ret2);

rdAng = (-eulRPY[1]) * TORAD;
mat[0]= cos(rdAng); mat[4]= 0.0 ; mat[8] = sin(rdAng);
mat[1]= 0.0 ; mat[5]= 1.0 ; mat[9] = 0.0 ;
mat[2]= -sin(rdAng); mat[6]= 0.0 ; mat[10]= cos(rdAng);

MatrXMatr33(mat_ret2, mat, mat_ret1);

rdAng = (-eulRPY[0]) * TORAD;
mat[0]= 1.0 ; mat[4]= 0.0 ; mat[8] = 0.0 ;
mat[1]= 0.0 ; mat[5]= cos(rdAng); mat[9] =-sin(rdAng) ;
mat[2]= 0.0 ; mat[6]= sin(rdAng); mat[10]= cos(rdAng) ;

MatrXMatr33(mat_ret1, mat, mat_ret2);

trasp_mat33(mat_ret2, mat_ret1);
} // *** End EulToDcm33() ***

int main(int argc, char *argv[])
{
double eulerRPY[4];
double matrix[16];

eulerRPY[2]=33.0;
eulerRPY[1]=44.0;
eulerRPY[0]=22.0;

printf("\nEuler (RPY) A=33.0 B=44.0 C=22.0\n\n");

EulToDcm33(eulerRPY, matrix);

printf("Matrix (column vectors):\n");
printf(" %f, %f, %f, %f\n", matrix[0], matrix[4], matrix[8] , matrix[12]);
printf(" %f, %f, %f, %f\n", matrix[1], matrix[5], matrix[9] , matrix[13]);
printf(" %f, %f, %f, %f\n", matrix[2], matrix[6], matrix[10], matrix[14]);
printf(" %f, %f, %f, %f\n", matrix[3], matrix[7], matrix[11], matrix[15]);

return (0);
}// *** End main() ***


Bye, rogxx.

[jenny_wui]

Could you tell me why you have added negative value for gimbal angles. Thank you in advance.

[rogxx]

I have tested this routine with Kuka robots (that uses Euler RPY angles, first A around Z, then B around Y, then C around X, in this order) and with a CAD program (CATIA, that use direction cosine matrices). In this situation the trasformation is OK!
May be some one with a better knowledge of mathematics than me can explain better this question.

Bye,
rogxx

[ugluk]

What he's doing is trivial. He's just multiplying the rotation matrices for rotating around principal axes. The negative angle means he's not reversing the rotation direction, e.g. anti-clockwise -> clockwise and vice versa.

[KjuEnnDee]

Hmm, the suggested code could in fact lead to problems as it is a combined transformation of three rotations. Imagine you first rotate 90deg around the X axis. If I'm not wrong this will swap the Y and Z axis. Afterwards a rotation around the Y axis is in fact a rotation around the Z axis and the third rotation around the Z axis (of the original system) is then a rotation around some other axis :).

To have more control (and consider the above situation) there is the so-called 3-1-3 rule, which says: 1. rotate X, 2. rotate Z, 3. rotate X. See the wikipedia article "Euler Angles" and the section "Euler angles as composition of extrinsic rotations" for more details.

In OpenGL such a rotation is simply done by:


glRotated(alpha, 1.0, 0.0, 0.0);
glRotated(beta, 0.0, 0.0, 1.0);
glRotated(gamma, 1.0, 0.0, 0.0);


Also be aware of the unholy "gimbal lock" which can be avoided by not using Euler angles but a free rotation axis and an angle.

Regards

[ugluk]

If you use the glRotate(), glTranslate(), ... functions, they are done on the CPU, they are not accelerated.

[KjuEnnDee]

Nobody has claimed something different. However, it integrates much more into a maintainable code than representing matrices - entry for entry - explicitly in a float array.

Xmas regards

[ugluk]

Float arrays are great, along with an appropriate C++ wrapper.

http://tvmet.sourceforge.net/

[rogxx]

I dont think that there are the problems KjuEnnDee wrote:


Hmm, the suggested code could in fact lead to problems as it is a combined transformation of three rotations. Imagine you first rotate 90deg around the X axis. If I'm not wrong this will swap the Y and Z axis. Afterwards a rotation around the Y axis is in fact a rotation around the Z axis and the third rotation around the Z axis (of the original system) is then a rotation around some other axis.

To have more control (and consider the above situation) there is the so-called 3-1-3 rule, which says: 1. rotate X, 2. rotate Z, 3. rotate X.


In my previous post i have written:


I have tested this routine with Kuka robots (that uses Euler RPY angles, first A around Z, then B around Y, then C around X, in this order)


There are many conventions to use Euler angles.
If You want to use them, You must know 4 elements:
the 3 angles (A, B, C) and the order of rotation around axis.

The system I used (also used by Kuka robots and by Sinumerik N/C machines controllers):
1) A around Z
2) B around Y
3) C around X

is as good as the system used by KjuEnnDee:
1) A around X
2) B around Z
3) C around X

Regards,
rogxx