Quaternions#
Quaternion Theory#
These are complex (real and imaginary) numbers in 3D space, they are made up from four real numbers q0,q1,q2 and q3 and three imaginary numbers i,j and k:
q = q0 + iq1 + jq2 + kq3
Think of i, j and k as being mutually orthogonal imaginary vectors of unit
length. As in a complex number i represents the square root of -1, so
j and k are also the square root of -1. This means that:
ii = -1
jj = -1
kk = -1
Nothing strange here, but what happens when different imaginary numbers are multiplied together?:
ij = k -- (1)
ji = -k -- (2)
ij = -ji -- (3) follows from (1) and (2)
ijk = -1 -- (4) follows from (1) times k
The normalized quaternion has unit length, so if q0² + q1² + q2² + q3² equals 1 then we have a normalized quaternion - we needn't take the square root since the square root of 1 is 1.
There are all sorts of conversions given for quaternion to Euler, Yaw Pitch and Roll out there, but this begs the question what about position vectors as in VPython. Some advocate changing to ypr then convert again to a vector. In the meantime any advantage of using quaternions to eliminate gimbal lock is lost.
Visualising the Quaternion#
First of all modify the sketch MPU6050_DMP6 so that we are reading quaternions. Save as MPU6050_DMP6Quaternion and our results look like the following:-
Show/Hide Code MPU6050_DMP6Quaternion.ino
// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project
#include "NewI2Cdev.h"
#include "NewMPU6050_6Axis_MotionApps20.h"
//#include "NewMPU6050.h" // not necessary if using MotionApps include file
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 mpu;
//MPU6050 mpu(0x69); // <-- use for AD0 high
/* =========================================================================
NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch
depends on the MPU-6050's INT pin being connected to the Arduino's
external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is
digital I/O pin 2.
* ========================================================================= */
// uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual
// quaternion components in a [w, x, y, z] format (not best for parsing
// on a remote host such as Processing or something though)
#define OUTPUT_READABLE_QUATERNION
#define INTERRUPT_PIN 2 // use pin 2 on Arduino Uno & most boards
#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
bool blinkState = false;
// MPU control/status vars
bool dmpReady = false; // set true if DMP init was successful
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount; // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer
// orientation/motion vars
Quaternion q; // [w, x, y, z] quaternion container
VectorFloat gravity; // [x, y, z] gravity vector
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
// ================================================================
// === INTERRUPT DETECTION ROUTINE ===
// ================================================================
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
mpuInterrupt = true;
}
// ================================================================
// === INITIAL SETUP ===
// ================================================================
void setup() {
// join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
//TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
// initialize serial communication
// (115200 chosen because it is required for Teapot Demo output, but it's
// really up to you depending on your project)
Serial.begin(115200);
while (!Serial); // wait for Leonardo enumeration, others continue immediately
// NOTE: 8MHz or slower host processors, like the Teensy @ 3.3v or Ardunio
// Pro Mini running at 3.3v, cannot handle this baud rate reliably due to
// the baud timing being too misaligned with processor ticks. You must use
// 38400 or slower in these cases, or use some kind of external separate
// crystal solution for the UART timer.
// initialize device
Serial.println(F("Initializing I2C devices..."));
mpu.initialize();
pinMode(INTERRUPT_PIN, INPUT);
// verify connection
Serial.println(F("Testing device connections..."));
Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
// wait for ready
//Serial.println(F("\nSend any character to begin DMP programming and demo: "));
//while (Serial.available() && Serial.read()); // empty buffer
//while (!Serial.available()); // wait for data
//while (Serial.available() && Serial.read()); // empty buffer again
// load and configure the DMP
Serial.println(F("Initializing DMP..."));
devStatus = mpu.dmpInitialize();
// supply your own gyro offsets here, scaled for min sensitivity
mpu.setXGyroOffset(1334);
mpu.setYGyroOffset(-2407);
mpu.setZGyroOffset(1659);
mpu.setXAccelOffset(-77);
mpu.setYAccelOffset(69);
mpu.setZAccelOffset(62);
// make sure it worked (returns 0 if so)
if (devStatus == 0) {
// Calibration Time: generate offsets and calibrate our MPU6050
mpu.CalibrateAccel(6);
mpu.CalibrateGyro(6);
mpu.PrintActiveOffsets();
// turn on the DMP, now that it's ready
Serial.println(F("Enabling DMP..."));
mpu.setDMPEnabled(true);
// enable Arduino interrupt detection
Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
Serial.print(digitalPinToInterrupt(INTERRUPT_PIN));
Serial.println(F(")..."));
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
// set our DMP Ready flag so the main loop() function knows it's okay to use it
Serial.println(F("DMP ready! Waiting for first interrupt..."));
dmpReady = true;
// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
// ERROR!
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
// (if it's going to break, usually the code will be 1)
Serial.print(F("DMP Initialization failed (code "));
Serial.print(devStatus);
Serial.println(F(")"));
}
// configure LED for output
pinMode(LED_PIN, OUTPUT);
}
// ================================================================
// === MAIN PROGRAM LOOP ===
// ================================================================
void loop() {
// if programming failed, don't try to do anything
if (!dmpReady) return;
// wait for MPU interrupt or extra packet(s) available
while (!mpuInterrupt && fifoCount < packetSize) {
// other program behavior stuff here
// .
// .
// .
// if you are really paranoid you can frequently test in between other
// stuff to see if mpuInterrupt is true, and if so, "break;" from the
// while() loop to immediately process the MPU data
// .
// .
// .
}
// reset interrupt flag and get INT_STATUS byte
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
// get current FIFO count
fifoCount = mpu.getFIFOCount();
// check for overflow (this should never happen unless our code is too inefficient)
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
// reset so we can continue cleanly
mpu.resetFIFO();
Serial.println(F("FIFO overflow!"));
// otherwise, check for DMP data ready interrupt (this should happen frequently)
} else if (mpuIntStatus & 0x02) {
// wait for correct available data length, should be a VERY short wait
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
// read a packet from FIFO
mpu.getFIFOBytes(fifoBuffer, packetSize);
// track FIFO count here in case there is > 1 packet available
// (this lets us immediately read more without waiting for an interrupt)
fifoCount -= packetSize;
#ifdef OUTPUT_READABLE_QUATERNION
// display quaternion values in easy matrix form: w x y z
mpu.dmpGetQuaternion(&q, fifoBuffer);
//Serial.print("quat\t");
Serial.print(q.w);
Serial.print(","); // was "\t"
Serial.print(q.x);
Serial.print(","); // was "\t"
Serial.print(q.y);
Serial.print(","); // was "\t"
Serial.println(q.z);
#endif
// blink LED to indicate activity
blinkState = !blinkState;
digitalWrite(LED_PIN, blinkState);
}
}
Running this you should see something like the following:-
Show/Hide Code quat.txt
Inion...
Revision @ user[16][6] = 4D
Resetting memory bank selection to 0...
>*ul
Initializing DMP...
Checking hardware revision...
Revision @ user[16][6] = 4D
Resetting memory bank selection to 0...
>*Initializing I2C devices...
Testing device connections...
MPU6050 connection successful
Initializing DMP...
Checking hardware revision...
Revision @ user[16][6] = 4D
Resetting memory bank selection to 0...
>**......>......
// X Accel Y Accel Z Accel X Gyro Y Gyro Z Gyro
//OFFSETS 1333, -2405, 1656, -48, 21, 94
Enabling DMP...
Enabling interrupt detection (Arduino external interrupt 0)...
0)...
DMP ready! Waiting for first interrupt...
1.00,0.00,-0.00,0.00
1.00,0.00,-0.00,0.00
1.00,0.00,-0.00,0.00
1.00,0.00,-0.00,0.00
1.00,0.00,-0.00,0.00
1.00,0.00,-0.00,0.00
1.00,0.00,-0.00,0.00
1.00,0.00,-0.00,0.00
1.00,0.00,-0.00,0.00
1.00,0.00,-0.00,0.00
1.00,0.00,-0.00,0.00
1.00,0.00,-0.00,0.00
1.00,0.00,-0.00,0.00
1.00,0.00,-0.00,0.00
First you should see the feedback about the start up of the IMU, then numbers to show the quaternions at the start, q0 is at 1.0 and the others are 0.0. The results look promising and would benefit from a customised plot from VPython. Import vpython, and serial. Set up the graph so that it is not fast, set the limits to y, the size and background colour. We expect that y will lie between 1 and -1, since both q0 and the sum of the squares start at 1.0 where we add a small clearance. Set a colour and name for each of the label variables.
Plotting the quaternion in vpython#
We want the data to be read continuously, even when the data is not yet ready so the first while loop - without it the python program gets kicked out. The second while loop checks that there is data waiting for us, the data is then parsed and we calculate the square of the inputs. Each input and the squared data is plotted against t.
After the readiness information the plotting begins, while the web page is being created you should see the q0 and square plots running at 1.0, the other 3 plots will be at 0.0. Pick up the module wave it around twisting and turning. It should plot continuously even when the module is turned upside down. What is noticeable is that once plotting there is not a lot of false data being generated, or at least detected, unlike yaw, pitch and roll. The data shows that the quaternions are normalised and produces stable data, but also the output does not precess when at rest.
It may take a try or two before it works, normally due to false data being detected when the computer and Arduino are not yet fully synchronised.
Show/Hide Code plot_quaternion.py
from vpython import *
from serial import Serial
ad=Serial('com3',115200) # this also sends a reset command
sleep(1) # let arduino wake up
gr = graph(fast=False, ymin=-1, ymax=1.1,width=800,height=600,xmin=0, xmax=10,
background=color.white)
f0 = gcurve(color=color.red,label="q0")
f1 = gcurve(color=color.green,label="q1")
f2 = gcurve(color=color.cyan,label="q2")
f3 = gcurve(color=color.orange,label="q3")
f4 = gcurve(color=color.magenta,label="square")
t=0 # time
dt=0.01 # time interval
while 'esc' not in keysdown():
while (ad.inWaiting()>0):
dataPacket=ad.readline()
dataPacket=str(dataPacket,'utf-8').strip()
splitPacket=dataPacket.split(",")
if len(splitPacket)==4:
try: # used to eliminate xxx.xx.xx
q0=float(splitPacket[0])
q1=float(splitPacket[1])
q2=float(splitPacket[2])
q3=float(splitPacket[3])
sq=q0*q0+q1*q1+q2*q2+q3*q3
if t > gr.xmax:
d = t-gr.xmax
gr.xmin += d
gr.xmax += d
f0.plot(t,q0)
f1.plot(t,q1)
f2.plot(t,q2)
f3.plot(t,q3)
f4.plot(t,sq)
t=t+dt
except ValueError:
print('data',dataPacket)
else:
print('split',splitPacket)
Rotating an enDAQ Sensor#
Using quaternions gimbal lock is no longer with us. The enDAQ sensor can transmit wirelessly, so the results are shown in this small video. The output is shown when rotating around all three axes.
Quaternion Rotating about all 3 Axes Pete Scheidler#
Now he rotates about a single axis 3 times, the results are plotted against Euler angles. If he had used Euler angles directly there would have been discontinuities at 180° and -180°, instead of the sinusoidal curves shown.
Quaternion Rotating one axis 3 times#
Axis-Angle to Quaternion#
Refer to Rotation Quaternions, and How to Use Them
According to the Euler rotation theorem any 3D rotation (or sequence of rotations) can be specified using two parameters: a unit vector that defines an axis of rotation and an angle ϴ describing the magnitude of the rotation about that axis.
An axis-angle rotation can therefore be represented by four numbers as in following equation:
(θ, x̂, ŷ, ẑ)
where (x̂, ŷ, ẑ) is a unit vector that defines the axis of rotation
A rotation quaternion is similar to an axis-angle representation. If we know the axis-angle components (θ, x̂, ŷ, ẑ), we can convert to a rotation quaternion q as follows:
q = (q0, q1, q2, q3)
where q0=cos(θ/2)
q1=x̂sin(θ/2)
q2=ŷsin(θ/2)
q3=ẑsin(θ/2)
From these equations we can see that the real term of the quaternion (q0) is completely determined by the rotation angle, and the remaining three imaginary terms (q1, q2 and q3) are just the three rotation axis vectors scaled by a common factor. The magnitude of a rotation quaternion (that is, the sum of the squares of all four components) is always equal to one.
Convert Quaternion to Axis-Angle#
First extract the rotation angle q0:
θ=2acos(q0)
if is θ not equal to 0
(x̂, ŷ, ẑ)=(q1/sin(θ/2),q2/sin(θ/2),q3/sin(θ/2))
The test condition is equivalent to q=(1,0,0,0) when θ is zero, and our module has not made any movement. In this case:
if q0 equals 1.0
θ=to 0
(x̂, ŷ, ẑ)=(1,0,0)
The values (x̂, ŷ, ẑ) can be used as our position vector, rotation is handled by:
v2 = v1.rotate(angle=theta,axis=vec(x̂, ŷ, ẑ))
Pretty straightforward - eh?
Creating a Glider#
Drawing a glider in VPython#
The standard example, Processing teapot, shows an aeroplane, so I thought to make a glider.
Show/Hide Code glider.py
from vpython import *
scene.title='My Glider'
scene.width=400
scene.height=400
scene.background=color.cyan
sr = scene.range=100
L = sr*0.75
r = 0.04*L
line=cylinder(pos=vec(-L,0,0),axis=vec(1,0,0),length=2*L,radius=r/5)
xarrow=arrow(pos=vec(-3/4*L,0,0),length=L/4, shaftwidth=r/2, color=color.red,
axis=vector(1,0,0))
yarrow=arrow(pos=vec(-3/4*L,0,0),length=L/2, shaftwidth=r/2, color=color.green,
axis=vector(0,1,0))
zarrow=arrow(pos=vec(-3/4*L,0,0),length=L/2, shaftwidth=r/2, color=color.blue,
axis=vector(0,0,1))
tube = cylinder(pos=vector(2*r-L/2.0,0,0), radius=r,length=L,
color=vec(1.0,0.7,0.2))
end = sphere( pos=vector(2*r-L/2.0,0,0), radius=r, color=vec(1.0,0.7,0.2))
end1 = sphere( pos=vector(2*r+L/2.0,0,0), radius=r, color=vec(1.0,0.7,0.2))
cockpit = sphere(pos=vector(4*r-L/2.0,0,0.6*r), radius=r, color=color.gray(0.8))
wing = ellipsoid(pos=vector(0.4*L-L/2.0,0,0),size=vec(0.24*L,2.24*L,r/5),
color=vec(1,0.32,0.33))
up = box(pos=vector(0.46*L,0,3.6*r),size=vec(4*r,r/5,6*r),color=vec(1,0.62,0))
tail = ellipsoid(pos=vector(0.46*L,0,5.4*r),size=vec(4*r,0.4*L,r/5),
color=vec(1,0.32,0.33))
plane = compound([tube,end,end1,cockpit,wing,up,tail])
currentaxis=vec(0,0,0)
currentangle=0
oldangle=0
def cachange(sl):
global currentangle
currentangle=sl.value*pi/180
sltext.text='{}'.format(sl.value)
#print(sl.value)
def xchange(sl):
xsltext.text='{:1.2f}'.format(sl.value)
def ychange(sl):
ysltext.text='{:1.2f}'.format(sl.value)
def zchange(sl):
zsltext.text='{:1.2f}'.format(sl.value)
# 0.262 radians is 30°
# The following statement will rotate the object named "obj" through an angle
# (measured in radians) about an axis relative to an origin:
sl=slider(min=0,max=360,length=300,value=0,step=1,bind=cachange)
scene.append_to_caption(' Current angle ')
sltext=wtext(text='{}'.format(sl.value))
scene.append_to_caption('\n\n')
xsl=slider(min=-1,max=1,length=300,value=0,step=0.01,bind=xchange)
scene.append_to_caption(' x value ')
xsltext=wtext(text='{:1.2f}'.format(xsl.value))
scene.append_to_caption('\n\n')
ysl=slider(min=-1,max=1,length=300,value=0,step=0.01,bind=ychange)
scene.append_to_caption(' y value ')
ysltext=wtext(text='{:1.2f}'.format(ysl.value))
scene.append_to_caption('\n\n')
zsl=slider(min=-1,max=1,length=300,value=0,step=0.01,bind=zchange)
scene.append_to_caption(' z value ')
zsltext=wtext(text='{:1.2f}'.format(zsl.value))
while 'esc' not in keysdown():
rate(50)
delta=currentangle-oldangle
oldangle=currentangle
plane.rotate(angle=delta,axis=vec(xsl.value,ysl.value,zsl.value)) # 0.261
#print(plane.pos)
This will be the basis of our visualisation. For demonstration purposes the glider can be turned and twisted by clicking on the right mouse button, in the quaternion script these properties are inactivated.
So the glider fuselage would not be a plain tube add a sphere to both ends of a cylinder. Add a wing, a thin ellipsoid and tailplane also made from a thin ellipsoid and box. The underside to the shapes have a different colour to the top colour, this is done automatically in vpython.
Add four sliders to show angle, and the axis x,y,z values. The angle is
shown in degrees but is used as radians in the rotate function. Vpython
widgets must be bound to a function, add a description of the slider and
show its value. Further information about vpython widgets is available at
Widgets.
When run the glider rotates and positions itself according to the axis
values, if all three axis values are zero nothing happens, also rotate
does not start unless the angle changes, just changing the axis values will
not directly affect the output.
Glider with Input for Rotate#
Using the Quaternions#
Create a new python script quaternion.py, this will have the glider we developed above (no need for the sliders) and a plot of the values.
Quaternion Glider#
It was found that the input values sometimes strayed beyond the upper limit,
so the inputs were constrained to be between 1.0 and -1.0, this removed most
of the false values being created. The speed was not constrained, some of
the vpython examples used rate(number) within the loop, increasing the
value then dispensing with it altogether increased the responsiveness. It is
important that the angle in rotate is the difference from the last value
and not the actual value or else the glider spins like a demented bluebottle.
If you are using fast=False in the graph, shutdown with esc then
disconnect, place the cursor on the x-axis values, the cursor should change
to a double headed horizontal arrow, move the cursor towards the lowest
x value. You should be able to see the hidden plots up to the origin.