Due to high demand expect some shipping delays at this time, orders may not ship for 1-2 business days.

## BNO085 Roll and yaw buggy when pith + or - 90 deg Moderators: adafruit_support_bill, adafruit

BNO085 Roll and yaw buggy when pith + or - 90 deg

Hi,

I recently purchased a BNO085 and hooked it up to my Teensy 4.0 using I2C.

I am getting the RotationVector from the sensor and transforming those quaternion values to euler.

I get good data from roll, pitch and yaw as long as I keep the sensor away from certain angles. As soon as I tilt pitch to 90 degrees (around Y-axis), roll and yaw start shooting all over the place. Tilting roll to 90 degrees does not have the same effect on pitch and yaw, so I still see those well.

roll to 90 degrees:
setting roll to 90 degrees and moving yaw and pitch
roll_90_deg.png (95.38 KiB) Viewed 193 times

pitch to 90 degrees:
setting pitch to 90 degrees and moving yaw and roll
pitch_90_deg.png (108.09 KiB) Viewed 193 times

Could anyone point me in the right direction?

StefanBonhof

Posts: 7
Joined: Thu Oct 28, 2021 10:25 am

Re: BNO085 Roll and yaw buggy when pith + or - 90 deg

That's normal behavior of Euler angles. When pitch is near zero, the individual Euler angles have some intuitive meaning, but the closer you get to the poles (pitch +90 or -90), the more important it is to consider all three angles simultaneously (because roll and yaw are teaming-up to confuse you).

Consider this analogy:
- You're wearing snowshoes, standing a couple steps from Earth's north pole (latitude almost +90).
- You're facing the pole with Canada behind you, so you're in the western hemisphere facing north.
- Walk forward a few steps (across the pole).
- You're now in the eastern hemisphere facing south!
- You moved only a tiny distance, yet your longitude and orientation angles have changed wildly.

Does that help?

Here's what I meant by "teaming-up". Notice the wild squiggly parts of your roll and yaw plots (circled in red) are very similar. The individual angles becomes erratic, but their difference becomes steady:
Attachments
pitch_90_deg_circled.png (121.47 KiB) Viewed 180 times

gammaburst

Posts: 629
Joined: Thu Dec 31, 2015 12:06 pm

Re: BNO085 Roll and yaw buggy when pith + or - 90 deg

Thank you for the elaborate and quick response!

I wrote a short python script in Blender to visualise the angles in one object, but I see the same behavior there.

Why does this only happen with pitch and not with roll (or yaw for that matter)? Rotating roll to + or - 90 deg does not have this effect.

The sensor has three axis magnetic field sensing so why is pitch behaving differently? Would using quaternions instead of Euler angles help?

StefanBonhof

Posts: 7
Joined: Thu Oct 28, 2021 10:25 am

Re: BNO085 Roll and yaw buggy when pith + or - 90 deg

Euler angles simply are not very useful, as they don't work at all under certain circumstances. And in the situations where they do work, Euler angles are not unique, in that there are always at least two sets of angles that give the same orientation.

Finally there are a dozen different definitions of Euler angles, because the sign and order of application of the individual rotations are not standardized, so it is impossible to answer your last question without knowing exactly which definition you are using.

That is why these days, almost everyone uses a quaternion or rotation matrix to represent a 3D orientation or transformation.

sj_remington

Posts: 190
Joined: Mon Jul 27, 2020 4:51 pm

Re: BNO085 Roll and yaw buggy when pith + or - 90 deg

StefanBonhof wrote:I wrote a short python script in Blender to visualise the angles in one object, but I see the same behavior there.

Are you sending the sensor's Euler data into Blender, and it's rotating and displaying a 3D object in real-time? I didn't know Blender could do that. That should work beautifully at all orientations. If your 3D object goes berserk at some orientations, then there's a math error somewhere, or you're doing the pitch/yaw/roll rotations in the wrong order. It's VERY easy to make such mistakes! Can you easily share your Python/Blender code? Maybe someone here can help you debug it. I have Blender and Python installed, but I've played with Blender for only about one day. I may be able to spot a math problem though.

I partially disagree with sj_remington. Euler angles can accurately describe any 3D orientation, and that's all Stefan is currently trying to do. Blender could probably accept the BNO's quaternions directly without any Euler conversion, however Stefan may have an unspoken requirement to learn or use Euler angles.

There are indeed many different Euler angle definitions, however I've seen only one of them in widespread use, the one used in aircraft/aerospace.

I'm going to poke around a bit with Blender ...

gammaburst

Posts: 629
Joined: Thu Dec 31, 2015 12:06 pm

Re: BNO085 Roll and yaw buggy when pith + or - 90 deg

I figured out Blender, Python, and serial communication.
As I rotate my BNO085, Blender rotates its default cube the same way. It works fine.

My Arduino reads the BNO085 ten times per second, and outputs serial text messages containing yaw roll pitch in degrees using aerospace conventions: positive yaw is nose right, positive roll is left wing up, positive pitch is nose up. Here's some example data:
20.82 -14.69 6.11
21.91 -12.11 6.44
23.52 -8.76 8.39
24.64 -6.72 10.25
25.33 -5.87 10.57
26.12 -3.64 12.21
25.89 -3.82 15.02

On my Windows 7 machine I installed pyserial 3.5 into Blender 2.92, and then ran this Python script. It runs for 100 messages (ten seconds) and then exits:
Code: Select all | TOGGLE FULL SIZE
`import serialimport timeimport bpyimport mathser = serial.Serial('COM11','115200')cube = bpy.context.selected_objects[0]for x in range(100):  s = ser.readline()  o = s.split()  yaw   = -math.pi/180 * float(o[0])  roll  = -math.pi/180 * float(o[1])  pitch =  math.pi/180 * float(o[2])  cube.rotation_euler = [roll, pitch, yaw]  bpy.ops.wm.redraw_timer(type='DRAW_WIN_SWAP', iterations=1)ser.close()`

gammaburst

Posts: 629
Joined: Thu Dec 31, 2015 12:06 pm

Re: BNO085 Roll and yaw buggy when pith + or - 90 deg

Euler angles can accurately describe any 3D orientation,
But, it is impossible to calculate a unique set of Euler angles to represent any given orientation, which vastly reduces their usefulness, and in cases where the singularities arise, the calculation fails completely. This thread is in fact a good example of the basic problem.

Most people fail to state the particular definition of Euler angles they are using, in which case their values are useless to others. Much like providing measurements without stating the units of measure.

sj_remington

Posts: 190
Joined: Mon Jul 27, 2020 4:51 pm

Re: BNO085 Roll and yaw buggy when pith + or - 90 deg

I'm not sure what you mean by "unique". If two or more different sets of Euler angles represent the same orientation, then that's bad?

Euler angles are often confusing to students, and calculations involving Euler angles can be messy, and the poles/singularities will bite you if you aren't careful with the math. Euler angles aren't going away anytime soon, so we must occasionally deal with them, if only to convert them to something simpler.

I agree that most people don't give enough info when discussing 3D orientation. I've seen far too many academic papers and software algorithms, and even product datasheets that don't clearly define the axes, or rotation directions, or rotation sequence.

gammaburst

Posts: 629
Joined: Thu Dec 31, 2015 12:06 pm

Re: BNO085 Roll and yaw buggy when pith + or - 90 deg

gammaburst wrote:I figured out Blender, Python, and serial communication.
As I rotate my BNO085, Blender rotates its default cube the same way. It works fine.

My Arduino reads the BNO085 ten times per second, and outputs serial text messages containing yaw roll pitch in degrees using aerospace conventions: positive yaw is nose right, positive roll is left wing up, positive pitch is nose up. Here's some example data:
20.82 -14.69 6.11
21.91 -12.11 6.44
23.52 -8.76 8.39
24.64 -6.72 10.25
25.33 -5.87 10.57
26.12 -3.64 12.21
25.89 -3.82 15.02

On my Windows 7 machine I installed pyserial 3.5 into Blender 2.92, and then ran this Python script. It runs for 100 messages (ten seconds) and then exits:
Code: Select all | TOGGLE FULL SIZE
`import serialimport timeimport bpyimport mathser = serial.Serial('COM11','115200')cube = bpy.context.selected_objects[0]for x in range(100):  s = ser.readline()  o = s.split()  yaw   = -math.pi/180 * float(o[0])  roll  = -math.pi/180 * float(o[1])  pitch =  math.pi/180 * float(o[2])  cube.rotation_euler = [roll, pitch, yaw]  bpy.ops.wm.redraw_timer(type='DRAW_WIN_SWAP', iterations=1)ser.close()`

I tried this and I got the same behavior as with my own code. Model flips weirdly when changing pitch close to 90 deg.

I didn't think orientation conventions mattered much since I simply had this issue that will occur no matter the convention.

I did however try to use quaternions instead of euler angles and that fixed the issue.

Code: Select all | TOGGLE FULL SIZE
`cube = bpy.context.selected_objects[0]cube.rotation_mode = 'QUATERNION'for x in range(10000):  s = ser.readline()  o = s.split()    x = float(o[0])  y = float(o[1])  z = float(o[2])  w = float(o[3])    cube.rotation_quaternion = [w, x, y, z] // Not sure yet if this is the order blender takes quaternion values but this works as a test setup nonetheless.  bpy.ops.wm.redraw_timer(type='DRAW_WIN_SWAP', iterations=1)  time.sleep(0.10)  ser.close()`

So my issue was, after all, caused by Euler angles being ambiguous in certain circumstances as sj_remmington mentioned.

StefanBonhof

Posts: 7
Joined: Thu Oct 28, 2021 10:25 am

Re: BNO085 Roll and yaw buggy when pith + or - 90 deg

Stefan, if you go away believing that, then you will take with you a false impression of Euler angles.

Your Euler implementation probably has a small bug in the Teensy code. If you'd like, I can help you debug it. Then you will see Euler working fine, exactly like the quaternion. If you share your Teensy code, then I can run it on my Teensy, feed its output into Blender, observe the problem, and fix the bug.

gammaburst

Posts: 629
Joined: Thu Dec 31, 2015 12:06 pm

Re: BNO085 Roll and yaw buggy when pith + or - 90 deg

I did however try to use quaternions instead of euler angles and that fixed the issue.

Good job. That is the modern approach. Faster, easier and far less ambiguous, too.

sj_remington

Posts: 190
Joined: Mon Jul 27, 2020 4:51 pm

Re: BNO085 Roll and yaw buggy when pith + or - 90 deg

Hi gammaburst,

thanks for looking out for me ;)

Code: Select all | TOGGLE FULL SIZE
`#include <SPI.h>#include <I2C.h>#include <Wire.h>#include <Adafruit_BNO08x.h>#define BNO08X_RESET -1EasyTransferI2C RX_I2C;float angles[3];// IMU setupAdafruit_BNO08x  bno08x(BNO08X_RESET);sh2_SensorValue_t sensorValue;struct euler_t {  float yaw;  float pitch;  float roll;} ypr;#ifdef FAST_MODE  // Top frequency is reported to be 1000Hz (but freq is somewhat variable)  sh2_SensorId_t reportType = SH2_GYRO_INTEGRATED_RV;  long reportIntervalUs = 2000;#else  // Top frequency is about 250Hz but this report is more accurate  sh2_SensorId_t reportType = SH2_ROTATION_VECTOR;  long reportIntervalUs = 5000;#endifvoid setReports(sh2_SensorId_t reportType, long report_interval) {  //Serial.println("Setting desired reports");  if (! bno08x.enableReport(reportType, report_interval)) {    //Serial.println("Could not enable stabilized remote vector");  }}void setup() {  // Begin I2C communications:  Wire.begin();  delay(50);  // IMU init:  bno08x.begin_I2C();  delay(10);  setReports(reportType, reportIntervalUs);  // Serial for testing  Serial.begin(115200);  delay(1000);}void quaternionToEuler(float qr, float qi, float qj, float qk, euler_t* ypr, bool degrees = false) {    float sqr = sq(qr);    float sqi = sq(qi);    float sqj = sq(qj);    float sqk = sq(qk);    ypr->yaw = atan2(2.0 * (qi * qj + qk * qr), (sqi - sqj - sqk + sqr));    ypr->pitch = asin(-2.0 * (qi * qk - qj * qr) / (sqi + sqj + sqk + sqr));    ypr->roll = atan2(2.0 * (qj * qk + qi * qr), (-sqi - sqj + sqk + sqr));    if (degrees) {      ypr->yaw *= RAD_TO_DEG;      ypr->pitch *= RAD_TO_DEG;      ypr->roll *= RAD_TO_DEG;    }}void quaternionToEulerGRV(sh2_RotationVector* rotational_vector, euler_t* ypr, bool degrees = false) {    quaternionToEuler(rotational_vector->real, rotational_vector->i, rotational_vector->j, rotational_vector->k, ypr, degrees);}void quaternionToEulerRV(sh2_RotationVectorWAcc_t* rotational_vector, euler_t* ypr, bool degrees = false) {    quaternionToEuler(rotational_vector->real, rotational_vector->i, rotational_vector->j, rotational_vector->k, ypr, degrees);}void quaternionToEulerGI(sh2_GyroIntegratedRV_t* rotational_vector, euler_t* ypr, bool degrees = false) {    quaternionToEuler(rotational_vector->real, rotational_vector->i, rotational_vector->j, rotational_vector->k, ypr, degrees);}void loop() {  delay(10);  // Read IMU.  if (bno08x.getSensorEvent(&sensorValue)) {    // in this demo only one report type will be received depending on FAST_MODE define (above)    switch (sensorValue.sensorId) {      case SH2_GAME_ROTATION_VECTOR:        quaternionToEulerGRV(&sensorValue.un.gameRotationVector, &ypr, true);      case SH2_GYRO_INTEGRATED_RV:        // faster (more noise?)        quaternionToEulerGI(&sensorValue.un.gyroIntegratedRV, &ypr, true);      case SH2_ROTATION_VECTOR:        quaternionToEulerRV(&sensorValue.un.rotationVector, &ypr, true);      break;    }  }  Serial.print(ypr.yaw);  Serial.print(" ");  Serial.print(ypr.pitch);  Serial.print(" ");  Serial.println(ypr.roll);}`

I think maybe the quaternion to euler calculations may be wrong, I did not write them myself but they seem different from what I find in other sources (like wikipedia lol). I tried to use other methods but they gave me the same behavior as before, so I don't know where I am going wrong.

StefanBonhof

Posts: 7
Joined: Thu Oct 28, 2021 10:25 am

Re: BNO085 Roll and yaw buggy when pith + or - 90 deg

so I don't know where I am going wrong.
In addition to all the ambiguities associated with the various Euler angle definitions, the equations you posted to calculate the angles do not take proper care with the special positions where singularities cause the calculation to go awry. Extra steps need to be added to correct for those occurrences.

Examples and possible solutions can be found in this nice exposition: http://www.euclideanspace.com/maths/geo ... onToEuler/

There are some useful comments in the last section, titled
Alternative Euler Angle Sequences

I am trying to make this site consistant and therefore I have standarised on one Euler angle sequence as defined on this page. However there are often reasons to use different Euler angle sequences:

You may be working with a program which uses a different Euler angle sequence.
You may need the singularities to be at different angles so they are less likely to cause problems.
The objects that you are modelling may just favor a different set of angles

sj_remington

Posts: 190
Joined: Mon Jul 27, 2020 4:51 pm

Re: BNO085 Roll and yaw buggy when pith + or - 90 deg

Diebel James (2006), Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors Stanford University
this is my favourite

jps2000

Posts: 686
Joined: Fri Jun 02, 2017 4:12 pm

Re: BNO085 Roll and yaw buggy when pith + or - 90 deg