Could anyone tell me how can I derive rotation matrix from gimbal angles? Thaks every body in advance.
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.
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.0174532925199432954743716805978692718781530857086181640625
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("
Euler (RPY) A=33.0 B=44.0 C=22.0
");
EulToDcm33(eulerRPY, matrix);
printf("Matrix (column vectors):
");
printf(" %f, %f, %f, %f
", matrix[0], matrix[4], matrix[8] , matrix[12]);
printf(" %f, %f, %f, %f
", matrix[1], matrix[5], matrix[9] , matrix[13]);
printf(" %f, %f, %f, %f
", matrix[2], matrix[6], matrix[10], matrix[14]);
printf(" %f, %f, %f, %f
", matrix[3], matrix[7], matrix[11], matrix[15]);
return (0);
}// *** End main() ***
Bye, rogxx.
Could you tell me why you have added negative value for gimbal angles. Thank you in advance.
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
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.
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
If you use the glRotate(), glTranslate(), … functions, they are done on the CPU, they are not accelerated.
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
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):
- A around Z
- B around Y
- C around X
is as good as the system used by KjuEnnDee:
- A around X
- B around Z
- C around X
Regards,
rogxx