Problem with Euler Angles

Olivier,

I found some problems with the Euler angle calculations this weekend.  I tested the previous equations by rotating the robot with the mouse, since I did not have a robot controller capable of turning on its own.   Apparently the rotation composition used by the mouse in the Webots program is different than the rotation composition used when a robot actually moves itself.  The current gps_euler( ) function in Webots 4.0.17 also returns wrong X and Z angles for some Y orientations of a robot.

Solution:
I looked at the VRML file and used a rotation composition based on the heirarchy of the robot.  
   Judoka_Robot {
      rotation 0 1 0 0 (rot about Y)
      children {
         back_1 servo {
            rotation 1 0 0 0 (rot about X)
            children {
               back_2 servo {
                  rotation 0 0 1 0 (rot about Z)
               }
            }
         }   
         gps_device
      }
   }

The resulting composition should be  Rgeneral = Ry * Rx * Rz instead of the previous  Rx * Ry * Rz.

The routine to compute the euler angles based on this composition is:
   public static float[] getAngles(float matrix[]) {
      
      float[] angles = new float[3];
      double cosX;

      angles[0] = (float)Math.asin(-1*matrix[9]); // find X angle

      cosX = Math.cos(angles[0]);
      angles[1] = (float)Math.atan2( -1*matrix[8]/cosX, matrix[10]/cosX  );  // find Y angle
      angles[2] = (float)Math.atan2( matrix[1]/cosX, matrix[5]/cosX );       // find Z angle

      return angles;
   } // end of getAngles()


I have generated a Judoka0 test robot that will rotate itself about Y and stop at 45 Degree intervals and bend forward,backward, left, and right, and display the computed angles from this new routine.
The back_1 and back_2 servos are set to a rotation of +/- 0.5 rad at each test point.
Hopefully, this routine will work in all cases, such as after Coyote does a summersalt and rotates X around a full 2*pi radians.


Judoka0 Test Robot:
   http://www.mikejost.com/cyberbotics/RotationMatrix/Judoka0_AngleTest.zip

HTML version of Maple worksheet of General Rotation Matrix for R=Ry*Rx*Rz:
   http://www.mikejost.com/cyberbotics/RotationMatrix/RotationMatrix.html


Mike Jost (FreeTIme)