In this lesson we show how to use quaternions from the BNO055 to create a visualization in Vpython. The visualization is a complete 3D free body rotation of a rigid body. To build this project you will need an Arduino Nano, and an Adafruit BNO055 Inertial Measurement Sensor.
This is the code we developed in the video posted here for your convenience. This code is for demo purposes only and should not be used in real applications. It is for educational purposes only.
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 |
#include <Wire.h> #include <Adafruit_Sensor.h> #include <Adafruit_BNO055.h> #include <utility/imumaths.h> #include <math.h> #define BNO055_SAMPLERATE_DELAY_MS (100) Adafruit_BNO055 myIMU = Adafruit_BNO055(); void setup() { // put your setup code here, to run once: Serial.begin(115200); myIMU.begin(); delay(1000); int8_t temp=myIMU.getTemp(); myIMU.setExtCrystalUse(true); } void loop() { // put your main code here, to run repeatedly: uint8_t system, gyro, accel, mg = 0; myIMU.getCalibration(&system, &gyro, &accel, &mg); imu::Quaternion quat=myIMU.getQuat(); Serial.print(quat.w()); Serial.print(","); Serial.print(quat.x()); Serial.print(","); Serial.print(quat.y()); Serial.print(","); Serial.print(quat.z()); Serial.print(","); Serial.print(accel); Serial.print(","); Serial.print(gyro); Serial.print(","); Serial.print(mg); Serial.print(","); Serial.println(system); delay(BNO055_SAMPLERATE_DELAY_MS); } |
This is the code we developed on the python side to do the visualization from the passed quaternions.
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 |
from vpython import * from time import * import numpy as np import math import serial ad=serial.Serial('com5',115200) sleep(1) scene.range=5 scene.background=color.yellow toRad=2*np.pi/360 toDeg=1/toRad scene.forward=vector(-1,-1,-1) scene.width=1200 scene.height=1080 xarrow=arrow(lenght=2, shaftwidth=.1, color=color.red,axis=vector(1,0,0)) yarrow=arrow(lenght=2, shaftwidth=.1, color=color.green,axis=vector(0,1,0)) zarrow=arrow(lenght=4, shaftwidth=.1, color=color.blue,axis=vector(0,0,1)) frontArrow=arrow(length=4,shaftwidth=.1,color=color.purple,axis=vector(1,0,0)) upArrow=arrow(length=1,shaftwidth=.1,color=color.magenta,axis=vector(0,1,0)) sideArrow=arrow(length=2,shaftwidth=.1,color=color.orange,axis=vector(0,0,1)) bBoard=box(length=6,width=2,height=.2,opacity=.8,pos=vector(0,0,0,)) bn=box(length=1,width=.75,height=.1, pos=vector(-.5,.1+.05,0),color=color.blue) nano=box(lenght=1.75,width=.6,height=.1,pos=vector(-2,.1+.05,0),color=color.green) myObj=compound([bBoard,bn,nano]) while (True): try: while (ad.inWaiting()==0): pass dataPacket=ad.readline() dataPacket=str(dataPacket,'utf-8') splitPacket=dataPacket.split(",") q0=float(splitPacket[0]) q1=float(splitPacket[1]) q2=float(splitPacket[2]) q3=float(splitPacket[3]) roll=-math.atan2(2*(q0*q1+q2*q3),1-2*(q1*q1+q2*q2)) pitch=math.asin(2*(q0*q2-q3*q1)) yaw=-math.atan2(2*(q0*q3+q1*q2),1-2*(q2*q2+q3*q3))-np.pi/2 rate(50) k=vector(cos(yaw)*cos(pitch), sin(pitch),sin(yaw)*cos(pitch)) y=vector(0,1,0) s=cross(k,y) v=cross(s,k) vrot=v*cos(roll)+cross(k,v)*sin(roll) frontArrow.axis=k sideArrow.axis=cross(k,vrot) upArrow.axis=vrot myObj.axis=k myObj.up=vrot sideArrow.length=2 frontArrow.length=4 upArrow.length=1 except: pass |