kopia lustrzana https://github.com/peterhinch/micropython-samples
Quaternions: initial commit.
Peter Hinch
rodzic
10f798ec4d
commit
8fd6341ca3
|
|
@ -0,0 +1,469 @@
|
|||
# 1. 3D Rotation (quaternions without the maths)
|
||||
|
||||
This repo contains a `Quaternion` class and a rudimentary 3D vector graphics
|
||||
module. The aim is to present quaternions as an efficient way to manipulate 3d
|
||||
objects and data from inertial measurement units (IMUs). As such they have
|
||||
applications in 3D graphics and robotics.
|
||||
|
||||
Quaternions have a reputation for being mathematically difficult. This repo
|
||||
aims to make them usable with no mathematics beyond a basic grasp of Cartesian
|
||||
(x, y, z) coordinates. They are actually easier to use than the traditional
|
||||
Euler angles (heading, pitch and roll), and avoid the mathematical problems
|
||||
associated with Euler angles.
|
||||
|
||||
The following fragment shows a classic demo where a wireframe cube rotates to
|
||||
match rotations of an IMU:
|
||||
|
||||
```python
|
||||
# dobj is a display list, imu is a BNo055
|
||||
# Create a unit cube, scale to 0.8 and move to centre on origin
|
||||
cube = Cube(RED, BLUE, GREEN) * ( 0.8, 0.8, 0.8) - (0.4, 0.4, 0.4)
|
||||
rot = Rotator() # Rotation quaternion
|
||||
while True:
|
||||
rot[:] = imu.quaternion() # Read IMU and assign to Rotator
|
||||
dobj['cube'] = cube @ rot # Rotate cube to match IMU
|
||||
dobj.show() # Display on screen
|
||||
time.sleep(0.1)
|
||||
```
|
||||
In this instance the IMU (a Bosch BNO055) produces quaternion data. Similar
|
||||
data may be computed from other IMUs by means of the
|
||||
[sensor fusion module](https://github.com/micropython-IMU/micropython-fusion).
|
||||
|
||||
Section 2 of this doc aims to describe basic usage of the repo with an absolute
|
||||
minimum of mathematics. Section 3 provides more detail for those wishing to
|
||||
expand the usage of quaternions beyond basic 3D transformations.
|
||||
|
||||
The 3D graphics is of demo quality: it is not a serious attempt at a graphics
|
||||
library. Its speed demonstrates that a 3D graphics library should be written in
|
||||
C for adequate performance. It also has known limitations and shortcomings. Its
|
||||
purpose is to prove and demonstrate the quaternion handling: all graphics
|
||||
elements use quaternions in their definitions.
|
||||
|
||||
# 2 Introduction
|
||||
|
||||
The graphics module enables various wireframe objects to be rendered from a
|
||||
"camera" view with an illusion of perspective. Objects may be moved, scaled,
|
||||
distorted and rotated using arithmetic operators. Thus if `cube` is an instance
|
||||
of a `Cube`,
|
||||
`cube + (0, 0.2, 0)` will move it along the Y axis.
|
||||
`cube * (1, 0.2, 0.2)` will turn it into a cuboid.
|
||||
`cube @ Rotator(0.1, 1, 0, 0)` will rotate it around the X axis by 0.1 radians.
|
||||
Any axis may be specified.
|
||||
|
||||
I wrote this to experiment with and test `quat.py` rather than as a serious
|
||||
attempt at 3D graphics. It is written in Python so speed is an issue. Realtime
|
||||
display of orientation is feasible. Playable games aren't. A C port would be
|
||||
awesome.
|
||||
|
||||
## 2.1 Files
|
||||
|
||||
The library provides the following files which should be copied to the target's
|
||||
filesystem.
|
||||
Core files:
|
||||
1. `quat.py` Provides classes for creating and rotating points.
|
||||
2. `graph3d.py` Provides wireframe 3D graphics classes.
|
||||
|
||||
Setup files. There should be a file `setup3d.py` defining graphics and
|
||||
(optionally) IMU hardware:
|
||||
1. `setup3d.py` Assumes an SSD1351 OLED display and BNo055 IMU.
|
||||
2. `setup3d_lcd160cr` Setup file for the official LCD. On the target rename to
|
||||
`setup3d.py` to use. Uncomment the BNo055 code if required.
|
||||
|
||||
Demo scripts:
|
||||
1. `test3d.py` Test script displays various moving objects.
|
||||
2. `test_imu.py` Wireframe cube moves to match IMU data.
|
||||
|
||||
Test:
|
||||
1. `quat_test.py` Unit test for `quat.py`. Run under Unix build.
|
||||
|
||||
## 2.2 Display hardware
|
||||
|
||||
The `setup3d.py` script assumes an SSD1351 128*128 bit display such as
|
||||
[this one](https://www.adafruit.com/product/1431) - it may be adapted for other
|
||||
displays. The driver is [here](https://github.com/peterhinch/micropython-nano-gui/blob/master/drivers/ssd1351/ssd1351_16bit.py)
|
||||
|
||||
The setup file must define the following data values:
|
||||
1. Color values in a format suitable for its `line` and `fill` functions (see
|
||||
below).
|
||||
2. `DIMENSION = 64` This defines the number of pixels moved by a value of 1.0.
|
||||
By default a 128*128 pixel space represents values defined by -1.0 <= v <= 1.0.
|
||||
3. `imu` Required for `test_imu`. An inertial measuring unit. It must have a
|
||||
`quaternion` method returning `w`, `x`, `y`, `z`. It may also have an `euler`
|
||||
method returning `heading`, `roll`, `pitch`. If `euler` is unsupported comment
|
||||
out the line in the test script that reports on those values.
|
||||
|
||||
The setup file must define these functions:
|
||||
1. `fill(color)` Clear the display when called with `BLACK`.
|
||||
2. `line(xs, ys, xe, ye, color)` Draw a line with args specified in absolute
|
||||
pixels.
|
||||
3. `show()` Display all lines.
|
||||
|
||||
By default the `imu` object is a BNo055 however it can be created for other IMU
|
||||
types by means of the
|
||||
[sensor fusion module](https://github.com/micropython-IMU/micropython-fusion)
|
||||
which provides access to quaternion, heading, pitch and roll.
|
||||
|
||||
The `test_imu` rotating cube demo uses the
|
||||
[Adafruit BNO055 breakout](https://www.adafruit.com/product/2472). The driver
|
||||
comprises these files:
|
||||
[bno055.py](https://github.com/micropython-IMU/micropython-bno055/blob/master/bno055.py)
|
||||
and [bno055_base.py](https://github.com/micropython-IMU/micropython-bno055/blob/master/bno055_base.py).
|
||||
|
||||
The `test3d.py` script ends by drawing a moving sphere. This is RAM-intensive
|
||||
and will cause a memory error unless run on a Pyboard D.
|
||||
|
||||
## 2.3 Getting started
|
||||
|
||||
The module `quat.py` provides functions producing two types of object:
|
||||
1. A `Point` representing a point (x, y, z) in 3D space.
|
||||
2. A `Rotator` representing an axis and an angle of rotation.
|
||||
|
||||
The following fragment instantiates a point and rotates it 45° around the X
|
||||
axis:
|
||||
```python
|
||||
from quat import Point, Rotator
|
||||
from math import pi
|
||||
point = Point(0, 0, 1)
|
||||
rot = Rotator(pi/4, 1, 0, 0) # Rotate 45° around x axis
|
||||
point @= rot # perform the rotation in-place
|
||||
```
|
||||
This can be extended to wireframe shapes, which can also be moved and scaled.
|
||||
Moving is done by adding a 3-tuple representing `(dx, dy, dz)` distances to
|
||||
move. Scaling may be done by multiplying by a value to make the object larger
|
||||
or smaller while preserving its shape. Alternatively the object can be
|
||||
multiplied by a 3-tuple to scale it differently along each axis:
|
||||
```python
|
||||
from quat import Rotator
|
||||
from math import pi
|
||||
from graph3d import Cube, DisplayDict, Axes
|
||||
from setup3d import * # Hardware setup and colors
|
||||
|
||||
# Objects for display are put in a `DisplayDict` which specifies the camera
|
||||
# location and distance for the perspective view
|
||||
dobj = DisplayDict(setup(), pi/6, 5)
|
||||
dobj['axes'] = Axes(WHITE) # Show x, y, z axes
|
||||
# By default the cube is from the origin to (1.0, 1.0, 1.0). Scale it
|
||||
# to length of side 0.8 and centre it at origin
|
||||
cube = Cube(RED, BLUE, GREEN) * (0.8, 0.8, 0.8) - (0.4, 0.4, 0.4)
|
||||
# Rotate 45° around an axis from (0, 0, 0) to (1, 1, 1)
|
||||
rot = Rotator(pi/4, 1, 1, 1)
|
||||
dobj['cube'] = cube @ rot
|
||||
dobj.show() # Update the physical display
|
||||
```
|
||||
|
||||
## 2.4 The quat module
|
||||
|
||||
This provides the following:
|
||||
1. `Point` Represents a point in space. Constructor takes `x`, `y` and `z`
|
||||
floating point args where `-1.0 <= arg <= 1.0`.
|
||||
2. `Rotator` An object which can rotate `Point` instances around an axis. The
|
||||
axis is defined by a line from the origin to a point. Constructor args:
|
||||
`theta=0, x=0, y=0, z=0` where `theta` is the amount of rotation in radians.
|
||||
3. `Euler` Returns a `Rotator` defined by Euler angles. Constructor args:
|
||||
`heading, pitch, roll` specified in radians.
|
||||
|
||||
## 2.5 The 3D graphics module
|
||||
|
||||
Code in this module uses `Point` and `Rotator` instances to create graphics
|
||||
primitives. Constructors create unit sized objects at a fixed location: objects
|
||||
may be scaled, distorted, moved or rotated prior to display.
|
||||
|
||||
In all cases `color` is a color value defined in `setup3d.py`.
|
||||
|
||||
The module provides the following objects:
|
||||
1. `Line` Constructor args: `p0, p1, color` where `p0` and `p1` are `Point`
|
||||
instances.
|
||||
2. `Axes` Draws the three axes. Constructor arg: `color`.
|
||||
3. `Square` Draws a unit square located in the +ve quadrant. Arg: `color`.
|
||||
4. `Circle` Unit circle centred on origin. Args: `color`, `segments=12`.
|
||||
5. `Sphere` Unit sphere centred on origin. Args: `color`, `segments=12`.
|
||||
6. `Cone` Unit cone with apex at origin. Args: `color`, `segments=12`.
|
||||
7. `Cube` Unit cube in +ve quadrant. Args: `color, front=None, sides=None`.
|
||||
These represent color values. If color values are passed it allows front, back
|
||||
and sides to have different colors enabling orientation to readily be seen.
|
||||
8. `DisplayDict` A dictionary of objects for display.
|
||||
|
||||
The `Sphere` object is demanding of RAM. I have only succeeded in displaying it
|
||||
on Pyboard D hosts (SF2 and SF6).
|
||||
|
||||
### 2.5.1 The DisplayDict class
|
||||
|
||||
This enables objects to be transformed or deleted at run time, and provides for
|
||||
display refresh.
|
||||
|
||||
Constructor:
|
||||
This takes the following args: `ssd, angle, distance`. `ssd` is the display
|
||||
instance. The other args specify the camera angle and distance. Typical values
|
||||
are pi/6 and 5.
|
||||
|
||||
Method:
|
||||
1. `show` Clears and refreshes the display.
|
||||
|
||||
# 3. Quaternions
|
||||
|
||||
The mathematics of quaternions are interesting. I don't propose to elaborate on
|
||||
it here as there are many excellent online texts and demonstrations. The ones I
|
||||
found helpful are listed [below](./quaternions.md#Appendix 1: references). The
|
||||
following summarises the maths necessary to read the rest of this section; it
|
||||
uses Python notation rather than trying to represent mathematical equations.
|
||||
|
||||
A quaternion comprises four numbers `w, x, y, z`. These define an instance
|
||||
`q = w + x*i + y*j + z*k` where `i, j, k` are square roots of -1 as per
|
||||
Hamilton's equation:
|
||||
`i**2 == j**2 == k**2 == i*j*k == -1`
|
||||
|
||||
A quaternion's `w` value is its scalar part, with `x`, `y` and `z` defining its
|
||||
vector part. A quaternion with `w == 0` is a vector quaternion and is created
|
||||
by the `Vector` or `Point` constructors in the code. It may be regarded as a
|
||||
point in space or a vector between the origin and that point, depending on
|
||||
context.
|
||||
|
||||
Quaternions have a magnitude represented by
|
||||
`magnitude = sqrt(sum((w*w + x*x + y*y + z*z)))` This may be accessed by
|
||||
issuing `magnitude = abs(q)`.
|
||||
|
||||
A quaternion whose vector part is 0 (i.e. x, y, z are 0) is a scalar. If w is 1
|
||||
it is known as a unit quaternion.
|
||||
|
||||
A quaternion with a nonzero vector part and a magnitude of 1 is a rotation
|
||||
quaternion. These may be produced by the `Rotator` constructor. If its `x y z`
|
||||
values are considered as a vector starting at the origin, `w` represents the
|
||||
amount of rotation about that axis.
|
||||
|
||||
Multiplication of quaternions is not commutative: in general
|
||||
`q1 * q2 != q2 * q1`
|
||||
Multiplication of quaternions produces a Hamilton product.
|
||||
|
||||
Quaternions have a conjugate analogous to the conjugate of a complex. If `q` is
|
||||
a quaternion its conjugate may be found with:
|
||||
`conj = Quaternion(q.w, -q.x, -q.y, -q.z)` In code `conj = q.conjugate()`.
|
||||
|
||||
A `Point` (vector quaternion) `P` may be rotated around an arbitrary axis by
|
||||
defining a rotation quaternion `R` and performing the following
|
||||
`P = R * P * conjugate(P)`
|
||||
This is performed using the matmul `@` operator, e.g.:
|
||||
`P @= R`
|
||||
The use of this operator allows rotations to be used in arithmetic expressions.
|
||||
|
||||
Division of quaternions is covered in [section 3.9](./QUATERNION.md#39-division).
|
||||
|
||||
## 3.1 The Quaternion class
|
||||
|
||||
This uses Python special ("dunder" or magic) methods to enable arithmetic and
|
||||
comparison operators and to support the iterator protocol.
|
||||
|
||||
```python
|
||||
q = Quaternion(5, 6, 7, 8)
|
||||
q2 = q * 2
|
||||
q2 = 2 * q # See NOTE on operand ordering
|
||||
print(q * 2 + (20, 30, 40))
|
||||
```
|
||||
Special constructors are provided to produce vector and rotation quaternions,
|
||||
with the class constructor enabling arbitrary quaternions to be created.
|
||||
|
||||
NOTE: Unless firmware is compiled with `MICROPY_PY_REVERSE_SPECIAL_METHODS`
|
||||
arithmetic expressions should have the quaternion on the left hand side. Thus
|
||||
`q * 2` will work, but `2 * q` will throw an exception on such builds. See
|
||||
[build notes](./QUATERNION.md#313-build-notes).
|
||||
|
||||
## 3.2 Quaternion constructors
|
||||
|
||||
There are constructors producing vector and rotation quaternions. In addition
|
||||
the actual class constructor enables any quaternion to be instantiated.
|
||||
|
||||
### 3.2.1 Vector or Point
|
||||
|
||||
These take 3 args `x, y, z`. The constructor returns a vector quaternion with
|
||||
`w == 0`.
|
||||
|
||||
### 3.2.2 Rotator
|
||||
|
||||
This takes 4 args `theta=0, x=0, y=0, z=0`. It returns a rotation quaternion
|
||||
with magnitude 1. The vector from the origin to `x, y, z` is the axis of
|
||||
rotation and `theta` is the angle in radians.
|
||||
|
||||
### 3.2.3 Euler
|
||||
|
||||
This takes 3 args `heading, pitch, roll` and returns a rotation quaternion. It
|
||||
aims to be based on Tait-Bryan angles, however this implementation assumes that
|
||||
positive `z` is upwards: some assume it is downwards. Euler angles have more
|
||||
standards than you can shake a stick at:
|
||||
[Euler angles are horrible](https://github.com/moble/quaternion/wiki/Euler-angles-are-horrible).
|
||||
|
||||
### 3.2.4 The Quaternion constructor
|
||||
|
||||
This takes four args `w=1, x=0, y=0, z=0`. The default is the unit quaternion.
|
||||
|
||||
## 3.3 Quaternion properties
|
||||
|
||||
Quaternion instances support `w, x, y, z` read/write properties.
|
||||
|
||||
## 3.4 Iterator protocol
|
||||
|
||||
Quaternion instances support this. Thus
|
||||
```python
|
||||
q = Quaternion(5, 6, 7, 8)
|
||||
print(q[1:]) # Prints the vector part array('f', [6.0, 7.0, 8.0])
|
||||
q[1:] = (9, 10, 11)
|
||||
print(q[1:]) # array('f', [9.0, 10.0, 11.0])
|
||||
for x in q:
|
||||
print(x) # It is possible to iterate over the elements
|
||||
```
|
||||
|
||||
## 3.5 abs() len() and str()
|
||||
|
||||
```python
|
||||
q = Quaternion(5, 6, 7, 8)
|
||||
abs(q) # Returns its magnitude 13.19090595827292
|
||||
len(q) # Always 4
|
||||
str(q) # 'w = 5.00 x = 6.00 y = 7.00 z = 8.00'
|
||||
```
|
||||
|
||||
## 3.6 Comparison operators
|
||||
|
||||
Two `Quaternion` instances are equal if all their `w, x, y, z` values match.
|
||||
Otherwise their maginitudes are compared. Thus `q1 == q2` tests for exact
|
||||
equality. `q1 >= q2` will return `True` if they are exactly equal or if
|
||||
`abs(q1) > abs(q2)`.
|
||||
|
||||
The test for "exact" equality is subject to the issues around comparison of
|
||||
floating point numbers. The `math.isclose` method is used with a value of 0.001
|
||||
(0.1%). This may be adjusted (`mdelta` and `adelta` variables).
|
||||
|
||||
## 3.7 Addition and subtraction
|
||||
|
||||
A `Quaternion` instance may have the following types added to it or subtracted
|
||||
from it, resulting in a new `Quaternion` instance being returned.
|
||||
1. Another Quaternion.
|
||||
2. A scalar. This modifies the `w` value.
|
||||
3. A 4-tuple or list: modifies `w, x, y, z`.
|
||||
4. A 3-tuple or list: modifies the vector part `x, y, z` and sets the scalar
|
||||
part `w` to zero returning a vector quaternion. Intended for moving `Point`
|
||||
instances.
|
||||
|
||||
The typical application of addition of a 3-tuple is the movement of a `Point`.
|
||||
|
||||
## 3.8 Multiplication
|
||||
|
||||
A `Quaternion` instance may be mutliplied by the following types resulting in a
|
||||
new `Quaternion` instance being returned.
|
||||
1. Another Quaternion. Returns the Hamilton product. Multiplication is not
|
||||
commutative.
|
||||
2. A scalar. This multiplies `w, x, y, z`.
|
||||
3. A 4-tuple or list: multiplies `w, x, y, z` by each element.
|
||||
4. A 3-tuple or list: multiplies the vector part `x, y, z` by each element and
|
||||
sets the scalar part `w` to zero returning a vector quaternion. Intended for
|
||||
scaling `Point` instances.
|
||||
|
||||
Multiplication by a scalar or a tuple is commutative. If a list of points
|
||||
comprises a shape, multiplication by a scalar can be used to resize the shape.
|
||||
Multiplication by a 3-tuple can transform a shape, for example changing a cube
|
||||
to a cuboid.
|
||||
|
||||
## 3.9 Division
|
||||
|
||||
It is possible to divide a scalar by a quaternion: `1/q` produces the inverse
|
||||
of quaternion `q`. This may also be acquired by issuing `q.inverse()`. The
|
||||
following statements are equivalent:
|
||||
```python
|
||||
q1 = (1, 2, 3) / Quaternion(4, 5, 6, 7)
|
||||
q2 = Quaternion(4, 5, 6, 7).inverse() * (1, 2, 3)
|
||||
```
|
||||
Note that multiplication of a quaternion by a scalar or by a tuple is
|
||||
commutative, while multiplication of quaternions is not. Consequently the
|
||||
notation `p/q` where `p` and `q` are quaternions is ambiguous because it is
|
||||
unclear whether it corresponds to `p*q.inverse()` or `q.inverse()*p`. The
|
||||
`Quaternion` class disallows this, raising a `ValueError`.
|
||||
|
||||
See [build notes](./QUATERNION.md#313-build-notes).
|
||||
|
||||
## 3.10 Rotation
|
||||
|
||||
Rotations are not commutative. Place two books side by side. Rotate one 90°
|
||||
clockwise, then flip it 180° about an axis facing away from you. Repeat with
|
||||
the other book in the opposite order.
|
||||
|
||||
Rotation is implemented using the matmul `@` operator. This is normally applied
|
||||
with a `Point` or `Vector` quaternion on the LHS and a `Rotator` (rotation
|
||||
quaternion) on the RHS.
|
||||
```python
|
||||
point = Point(1, 0, 0)
|
||||
rot = Rotator(pi/6, 0, 1, 0)
|
||||
new_point = point @ rot
|
||||
point @= rot # Rotate in place
|
||||
```
|
||||
There is an `rrot` method performing order-reversed rotation:
|
||||
```
|
||||
def __matmul__(self, rot):
|
||||
return rot * self * rot.conjugate()
|
||||
|
||||
def rrot(self, rot):
|
||||
return rot.conjugate() * self * rot
|
||||
```
|
||||
|
||||
### 3.10.1 Rotation direction
|
||||
|
||||
Consider these `Rotator` instances:
|
||||
```python
|
||||
rotx = Rotator(0.1, 1, 0, 0) # About X
|
||||
roty = Rotator(0.1, 0, 1, 0) # Y
|
||||
rotz = Rotator(0.1, 0, 0, 1) # Z
|
||||
point = Point(1, 1, 1)
|
||||
```
|
||||
Applying a rotator with a positive angle to a point will cause anticlockwise
|
||||
rotation as seen from a positive location on the relevant axis looking towards
|
||||
the origin. This is analogous to 2d rotation on the complex plane where
|
||||
multiplying by (say) 1 + 1j will produce an anticlockwise rotation.
|
||||
|
||||
## 3.11 Other methods
|
||||
|
||||
1. `conjugate` No args, returns a new `Quaternion` being its conjugate.
|
||||
2. `inverse` No args, returns a new `Quaternion` being its inverse.
|
||||
3. `to_angle_axis` No args. Intended for rotation quaternions. Returns
|
||||
`[theta, x, y, z]` where `theta` is the rotation angle and the vector from the
|
||||
origin to `x, y, z` is the axis.
|
||||
4. `copy` No args. Returns a copy of the instance.
|
||||
5. `normalise` Aims to produce a rotation quaternion: returns a new quaternion
|
||||
instance whose maginitude is 1. Intended to compensate for any accumulation of
|
||||
errors in a rotation quaternion.
|
||||
6. `isvec` No args. Returns `True` if it is a vector quaternion i.e. `w == 0`.
|
||||
7. `isrot` No args. Returns `True` if it is a rotation quaternion i.e.
|
||||
`abs(q) == 1`.
|
||||
|
||||
## 3.12 The euler function
|
||||
|
||||
This takes a `Quaternion` instance as an arg. Returns `heading, pitch, roll`.
|
||||
See [section 3.2.3](./quaternions.md#323-euler).
|
||||
|
||||
## 3.13 Build notes
|
||||
|
||||
Not all ports provide `math.isclose` and the reverse special methods. To some
|
||||
extent the lack of reverse special methods can be worked around by changing the
|
||||
operand order in expressions to ensure that the left hand side of a binary
|
||||
operator is a `Quaternion` instance. This cannot be applied to division and the
|
||||
`.inverse()` method must be used.
|
||||
|
||||
Bettrr, edit `ports/port/mpconfigport.h` to ensure these lines are included:
|
||||
```C
|
||||
#define MICROPY_PY_MATH_ISCLOSE (1)
|
||||
|
||||
#define MICROPY_PY_ALL_SPECIAL_METHODS (1) // May already be present
|
||||
#define MICROPY_PY_REVERSE_SPECIAL_METHODS (1) // Add this line
|
||||
```
|
||||
and rebuild the firmware.
|
||||
|
||||
# Appendix 1: references
|
||||
|
||||
[Visalising quaternions](https://www.youtube.com/watch?v=d4EgbgTm0Bg)
|
||||
[Visalising quaternions - interactive](https://eater.net/quaternions/video/doublecover)
|
||||
[Using Quaternion to Perform 3D rotations](https://www.cprogramming.com/tutorial/3d/uses.html)
|
||||
The Wikipedia article is maths-heavy.
|
||||
[Wikpedia](https://en.wikipedia.org/wiki/Quaternion)
|
||||
The following ref is good, but its formula for multiplication reverses the
|
||||
order of its arguments compared to the other sources. The q1*q2 from
|
||||
other sources corresponds to q2*q1 from this paper.
|
||||
[Rotation Quaternions, and How to Use Them](http://danceswithcode.net/engineeringnotes/quaternions/quaternions.html)
|
||||
|
||||
From the youtube video: p -> (q2(q1pq1**-1)q2**-1)
|
||||
|
||||
Why to use quaternions rather than [Euler angles](https://github.com/moble/quaternion/wiki/Euler-angles-are-horrible).
|
||||
34
README.md
34
README.md
|
|
@ -11,10 +11,10 @@ Pyboard variants.
|
|||
1.4 [Buildcheck](./README.md#14-buildcheck) Check firmware build date
|
||||
2. [Hardware information and drivers](./README.md#2-hardware-information-and-drivers)
|
||||
2.1 [ESP32](./README.md#21-esp32) Pinout and notes on the reference board
|
||||
2.2 [SSD1306](./README.md#22-ssd1306) Write large fonts to the SSD1306
|
||||
2.3 [Pyboard D](./README.md#23-pyboard-d) Assorted scraps of information
|
||||
2.4 [DS3231 precision RTC](./README.md#24-ds3231-precision-rtc) Use cheap hardware to calibrate Pyboard RTC
|
||||
3. [Essays](./README.md#3-essays) General thoughts
|
||||
2.2 [SSD1306](./README.md#22-ssd1306) Write large fonts to the SSD1306.
|
||||
2.3 [Pyboard D](./README.md#23-pyboard-d) Assorted scraps of information.
|
||||
2.4 [DS3231 precision RTC](./README.md#24-ds3231-precision-rtc) Use cheap hardware to calibrate Pyboard RTC.
|
||||
3. [Essays](./README.md#3-essays) General thoughts.
|
||||
3.1 [Resilient](./README.md#31-resilient) A guide to writing resilient WiFi code
|
||||
3.2 [Serialisation](./README.md#32-serialisation) Review of MicroPython's four serialisation libraries
|
||||
3.3 [Measurement of relative timing and phase of fast analog signals](./README.md#33-measurement-of-relative-timing-and-phase-of-fast-analog-signals) For Pyboard.
|
||||
|
|
@ -25,12 +25,13 @@ Pyboard variants.
|
|||
4.4 [Reverse](./README.md#44-reverse) Reversal algorithms for bytearrays
|
||||
4.5 [Timed function](./README.md#45-timed-function) Time execution with a decorator
|
||||
4.6 [ESP8266 MQTT benchmark](./README.md#46-esp8266-mqtt-benchmark) Test performance of MQTT with official library
|
||||
4.7 [Rotary incremental encoder](./README.md#47-rotary-incremental-encoder) Fast, simple, proven algorithm
|
||||
4.7 [Rotary incremental encoder](./README.md#47-rotary-incremental-encoder) Fast, simple, proven algorithm.
|
||||
4.8 [A pseudo random number generator](./README.md#48-a-pseudo-random-number-generator)
|
||||
4.9 [Verifying incrementing sequences](./README.md#49-verifying-incrementing-sequences) Test communications drivers
|
||||
4.10 [Bitmaps](./README.md#410-bitmaps) Non-allocating ways to access bitmaps
|
||||
4.11 [Functors and singletons](./README.md#411-functors-and-singletons) Useful decorators
|
||||
4.12 [A Pyboard power meter](./README.md#412-a-pyboard-power-meter) One of my own projects
|
||||
4.9 [Verifying incrementing sequences](./README.md#49-verifying-incrementing-sequences) Test communications drivers.
|
||||
4.10 [Bitmaps](./README.md#410-bitmaps) Non-allocating ways to access bitmaps.
|
||||
4.11 [Functors and singletons](./README.md#411-functors-and-singletons) Useful decorators.
|
||||
4.12 [Quaternions](./README.md#412-quaternions) Scale, move and rotate 3D objects with minimal mathematics.
|
||||
4.13 [A Pyboard power meter](./README.md#413-a-pyboard-power-meter) One of my own projects.
|
||||
|
||||
# 1. Installation guides
|
||||
|
||||
|
|
@ -240,7 +241,20 @@ subsequent accesses being via `__call__`. As an object it can retain state. As
|
|||
an example, a functor might have a continuously running task: successive calls
|
||||
modify the behaviour of the task.
|
||||
|
||||
# 4.12 A pyboard power meter
|
||||
## 4.12 Quaternions
|
||||
|
||||
Quaternions have a reputation for being mathematically difficult. Surely true
|
||||
if you intend using them to write Maxwell's equations; if your ambitions are
|
||||
limited to manipulating three dimensional objects or processing IMU data they
|
||||
can be remarkably simple.
|
||||
|
||||
The `Quaternion` class lets you create, move, transform and rotate 3D objects
|
||||
with no maths beyond familiarity with an `xyz` coordinate system. Includes a
|
||||
demo where a wireframe cube rotates in response to movements of a BNo055 IMU,
|
||||
plus a demo of moving wireframe graphics. Neither demo uses trig functions.
|
||||
See [the docs](./QUATERNIONS.md).
|
||||
|
||||
## 4.13 A pyboard power meter
|
||||
|
||||
This uses a Pyboard to measure the power consumption of mains powered devices.
|
||||
Unlike simple commercial devices it performs a true vector (phasor) measurement
|
||||
|
|
|
|||
|
|
@ -0,0 +1,190 @@
|
|||
# graph3d.py 3D graphics demo of quaternion use
|
||||
|
||||
# Released under the MIT License (MIT). See LICENSE.
|
||||
# Copyright (c) 2020 Peter Hinch
|
||||
|
||||
import gc
|
||||
from math import pi
|
||||
from quat import Rotator, Point
|
||||
from setup3d import fill, line, show, DIMENSION
|
||||
|
||||
class Line:
|
||||
def __init__(self, p0, p1, color):
|
||||
#assert p0.isvec() and p1.isvec()
|
||||
self.start = p0
|
||||
self.end = p1
|
||||
self.color = color
|
||||
|
||||
def show(self, ssd):
|
||||
_, xs, ys, zs = self.start
|
||||
_, xe, ye, ze = self.end
|
||||
# Handle perspective and scale to display
|
||||
w = DIMENSION # Viewing area is square
|
||||
h = w
|
||||
xs = round((1 + xs/zs) * w)
|
||||
ys = round((1 - ys/zs) * h)
|
||||
xe = round((1 + xe/ze) * w)
|
||||
ye = round((1 - ye/ze) * h)
|
||||
line(xs,ys, xe, ye, self.color)
|
||||
|
||||
def __add__(self, to): # to is a Point or 3-tuple
|
||||
return Line(self.start + to, self.end + to, self.color)
|
||||
|
||||
def __sub__(self, v): # to is a Point or 3-tuple
|
||||
return Line(self.start - v, self.end - v, self.color)
|
||||
|
||||
def __mul__(self, by): # by is a 3-tuple
|
||||
return Line(self.start * by, self.end * by, self.color)
|
||||
|
||||
def __matmul__(self, rot): # rot is a rotation quaternion
|
||||
#assert rot.isrot()
|
||||
return Line(self.start @ rot, self.end @ rot, self.color)
|
||||
|
||||
def camera(self, rot, distance): # rot is a rotation quaternion, distance is scalar
|
||||
#assert rot.isrot()
|
||||
gc.collect()
|
||||
ps = self.start @ rot
|
||||
ps = Point(ps.x * distance, ps.y * distance, distance - ps.z)
|
||||
pe = self.end @ rot
|
||||
pe = Point(pe.x * distance, pe.y * distance, distance - pe.z)
|
||||
return Line(ps, pe, self.color)
|
||||
|
||||
def __str__(self):
|
||||
return 'start {} end {}'.format(self.start, self.end)
|
||||
|
||||
class Shape:
|
||||
def __init__(self, lines):
|
||||
self.lines = lines
|
||||
|
||||
def __add__(self, to):
|
||||
return Shape([l + to for l in self.lines])
|
||||
|
||||
def __sub__(self, v):
|
||||
return Shape([l - v for l in self.lines])
|
||||
|
||||
def __mul__(self, by):
|
||||
return Shape([l * by for l in self.lines])
|
||||
|
||||
def __matmul__(self, rot):
|
||||
l = []
|
||||
for line in self.lines:
|
||||
l.append(line @ rot)
|
||||
return Shape(l)
|
||||
|
||||
def camera(self, rot, distance):
|
||||
l = []
|
||||
for line in self.lines:
|
||||
l.append(line.camera(rot, distance))
|
||||
return Shape(l)
|
||||
|
||||
def show(self, ssd):
|
||||
for line in self.lines:
|
||||
line.show(ssd)
|
||||
|
||||
def __str__(self):
|
||||
r = ''
|
||||
for line in self.lines:
|
||||
r = ''.join((r, '{}\n'.format(line)))
|
||||
return r
|
||||
|
||||
class Axes(Shape):
|
||||
def __init__(self, color):
|
||||
l = (Line(Point(-1.0, 0, 0), Point(1.0, 0, 0), color),
|
||||
Line(Point(0, -1.0, 0), Point(0, 1.0, 0), color),
|
||||
Line(Point(0, 0, -1.0), Point(0, 0, 1.0), color))
|
||||
super().__init__(l)
|
||||
|
||||
class Square(Shape): # Unit square in XY plane
|
||||
def __init__(self, color): # Corner located at origin
|
||||
l = (Line(Point(0, 0, 0), Point(1, 0, 0), color),
|
||||
Line(Point(1, 0, 0), Point(1, 1, 0), color),
|
||||
Line(Point(1, 1, 0), Point(0, 1, 0), color),
|
||||
Line(Point(0, 1, 0), Point(0, 0, 0), color))
|
||||
super().__init__(l)
|
||||
|
||||
class Cube(Shape):
|
||||
def __init__(self, color, front=None, sides=None): # Corner located at origin
|
||||
front = color if front is None else front
|
||||
sides = color if sides is None else sides
|
||||
l = (Line(Point(0, 0, 0), Point(1, 0, 0), color),
|
||||
Line(Point(1, 0, 0), Point(1, 1, 0), color),
|
||||
Line(Point(1, 1, 0), Point(0, 1, 0), color),
|
||||
Line(Point(0, 1, 0), Point(0, 0, 0), color),
|
||||
Line(Point(0, 0, 1), Point(1, 0, 1), front),
|
||||
Line(Point(1, 0, 1), Point(1, 1, 1), front),
|
||||
Line(Point(1, 1, 1), Point(0, 1, 1), front),
|
||||
Line(Point(0, 1, 1), Point(0, 0, 1), front),
|
||||
Line(Point(0, 0, 0), Point(0, 0, 1), sides),
|
||||
Line(Point(1, 0, 0), Point(1, 0, 1), sides),
|
||||
Line(Point(1, 1, 0), Point(1, 1, 1), sides),
|
||||
Line(Point(0, 1, 0), Point(0, 1, 1), sides),
|
||||
)
|
||||
super().__init__(l)
|
||||
|
||||
class Cone(Shape):
|
||||
def __init__(self, color, segments=12):
|
||||
rot = Rotator(2*pi/segments, 0, 1, 0)
|
||||
p0 = Point(1, 1, 0)
|
||||
p1 = p0.copy()
|
||||
orig = Point(0, 0, 0)
|
||||
lines = []
|
||||
for _ in range(segments + 1):
|
||||
p1 @= rot
|
||||
lines.append(Line(p0, p1, color))
|
||||
lines.append(Line(orig, p0, color))
|
||||
p0 @= rot
|
||||
super().__init__(lines)
|
||||
|
||||
class Circle(Shape): # Unit circle in XY plane centred on origin
|
||||
def __init__(self, color, segments=12):
|
||||
rot = Rotator(2*pi/segments, 0, 1, 0)
|
||||
p0 = Point(1, 0, 0)
|
||||
p1 = p0.copy()
|
||||
lines = []
|
||||
for _ in range(segments + 1):
|
||||
p1 @= rot
|
||||
lines.append(Line(p0, p1, color))
|
||||
p0 @= rot
|
||||
super().__init__(lines)
|
||||
|
||||
class Sphere(Shape): # Unit sphere in XY plane centred on origin
|
||||
def __init__(self, color, segments=12):
|
||||
lines = []
|
||||
s = Circle(color)
|
||||
xrot = Rotator(2 * pi / segments, 1, 0, 0)
|
||||
for _ in range(segments / 2 + 1):
|
||||
gc.collect()
|
||||
lines.extend(s.lines[:])
|
||||
s @= xrot
|
||||
super().__init__(lines)
|
||||
|
||||
|
||||
# Composition rather than inheritance as MP can't inherit builtin types.
|
||||
class DisplayDict:
|
||||
def __init__(self, ssd, angle, distance):
|
||||
self.ssd = ssd
|
||||
self.distance = distance # scalar
|
||||
# Rotation quaternion for camera view
|
||||
self.crot = Rotator(angle, 1, 1, 0)
|
||||
self.d = {}
|
||||
|
||||
def __setitem__(self, key, value):
|
||||
if not isinstance(value, Shape):
|
||||
raise ValueError('DisplayDict entries must be Shapes')
|
||||
self.d[key] = value
|
||||
|
||||
def __getitem__(self, key):
|
||||
return self.d[key]
|
||||
|
||||
def __delitem__(self, key):
|
||||
del self.d[key]
|
||||
|
||||
def show(self):
|
||||
ssd = self.ssd
|
||||
fill(0)
|
||||
crot = self.crot
|
||||
dz = self.distance
|
||||
for shape in self.d.values():
|
||||
s = shape.camera(crot, dz)
|
||||
s.show(ssd)
|
||||
show()
|
||||
|
|
@ -0,0 +1,243 @@
|
|||
# quat.py "Micro" Quaternion class
|
||||
|
||||
# Released under the MIT License (MIT). See LICENSE.
|
||||
# Copyright (c) 2020 Peter Hinch
|
||||
|
||||
from math import sqrt, sin, cos, acos, isclose, asin, atan2, pi
|
||||
from array import array
|
||||
mdelta = 0.001 # 0.1% Minimum difference considered significant for graphics
|
||||
adelta = 0.001 # Absolute tolerance for components near 0
|
||||
|
||||
def _arglen(arg):
|
||||
length = 0
|
||||
try:
|
||||
length = len(arg)
|
||||
except TypeError:
|
||||
pass
|
||||
if length not in (0, 3, 4):
|
||||
raise ValueError('Sequence length must be 3 or 4')
|
||||
return length
|
||||
|
||||
# Convert a rotation quaternion to Euler angles. Beware:
|
||||
# https://github.com/moble/quaternion/wiki/Euler-angles-are-horrible
|
||||
def euler(q): # Return (heading, pitch, roll)
|
||||
if not q.isrot():
|
||||
raise ValueError('Must be a rotation quaternion.')
|
||||
w, x, y, z = q
|
||||
pitch = asin(2*(w*y - x*z))
|
||||
if isclose(pitch, pi/2, rel_tol = mdelta):
|
||||
return -2 * atan2(x, w), pitch, 0
|
||||
if isclose(pitch, -pi/2, rel_tol = mdelta):
|
||||
return 2 * atan2(x, w), pitch, 0
|
||||
roll = atan2(2*(w*x + y*z), w*w - x*x - y*y + z*z)
|
||||
#roll = atan2(2*(w*x + y*z), 1 - 2*(x*x + y*y))
|
||||
hdg = atan2(2*(w*z + x*y), w*w + x*x - y*y - z*z)
|
||||
#hdg = atan2(2*(w*z + x*y), 1 - 2 *(y*y + z*z))
|
||||
return hdg, pitch, roll
|
||||
|
||||
|
||||
class Quaternion:
|
||||
|
||||
def __init__(self, w=1, x=0, y=0, z=0): # Default: the identity quaternion
|
||||
self.d = array('f', (w, x, y, z))
|
||||
|
||||
@property
|
||||
def w(self):
|
||||
return self[0]
|
||||
|
||||
@w.setter
|
||||
def w(self, v):
|
||||
self[0] = v
|
||||
|
||||
@property
|
||||
def x(self):
|
||||
return self[1]
|
||||
|
||||
@x.setter
|
||||
def x(self, v):
|
||||
self[1] = v
|
||||
|
||||
@property
|
||||
def y(self):
|
||||
return self[2]
|
||||
|
||||
@y.setter
|
||||
def y(self, v):
|
||||
self[2] = v
|
||||
|
||||
@property
|
||||
def z(self):
|
||||
return self[3]
|
||||
|
||||
@z.setter
|
||||
def z(self, v):
|
||||
self[3] = v
|
||||
|
||||
def normalise(self):
|
||||
if self[0] == 1: # acos(1) == 0. Identity quaternion: no rotation
|
||||
return Quaternion(1, 0, 0, 0)
|
||||
m = abs(self) # Magnitude
|
||||
assert m > 0.1 # rotation quaternion should have magnitude ~= 1
|
||||
if isclose(m, 1.0, rel_tol=mdelta):
|
||||
return self # No normalisation necessary
|
||||
return Quaternion(*(a/m for a in self))
|
||||
|
||||
def __getitem__(self, key):
|
||||
return self.d[key]
|
||||
|
||||
def __setitem__(self, key, v):
|
||||
try:
|
||||
v1 = array('f', v)
|
||||
except TypeError: # Scalar
|
||||
v1 = v
|
||||
self.d[key] = v1
|
||||
|
||||
def copy(self):
|
||||
return Quaternion(*self)
|
||||
|
||||
def __abs__(self): # Return magnitude
|
||||
return sqrt(sum((d*d for d in self)))
|
||||
|
||||
def __len__(self):
|
||||
return 4
|
||||
# Comparison: == and != perform equality test of all elements
|
||||
def __eq__(self, other):
|
||||
return all((isclose(a, b, rel_tol=mdelta, abs_tol=adelta) for a, b in zip(self, other)))
|
||||
|
||||
def __ne__(self, other):
|
||||
return not self == other
|
||||
|
||||
# < and > comparisons compare magnitudes.
|
||||
def __gt__(self, other):
|
||||
return abs(self) > abs(other)
|
||||
|
||||
def __lt__(self, other):
|
||||
return abs(self) < abs(other)
|
||||
|
||||
# <= and >= return True for complete equality otherwise magnitudes are compared.
|
||||
def __ge__(self, other):
|
||||
return True if self == other else abs(self) > abs(other)
|
||||
|
||||
def __le__(self, other):
|
||||
return True if self == other else abs(self) < abs(other)
|
||||
|
||||
def to_angle_axis(self):
|
||||
q = self.normalise()
|
||||
if isclose(q[0], 1.0, rel_tol = mdelta):
|
||||
return 0, 1, 0, 0
|
||||
theta = 2*acos(q[0])
|
||||
s = sin(theta/2)
|
||||
return [theta] + [a/s for a in q[1:]]
|
||||
|
||||
def conjugate(self):
|
||||
return Quaternion(self[0], *(-a for a in self[1:]))
|
||||
|
||||
def inverse(self): # Reciprocal
|
||||
return self.conjugate()/sum((d*d for d in self))
|
||||
|
||||
def __str__(self):
|
||||
return 'w = {:4.2f} x = {:4.2f} y = {:4.2f} z = {:4.2f}'.format(*self)
|
||||
|
||||
def __pos__(self):
|
||||
return Quaternion(*self)
|
||||
|
||||
def __neg__(self):
|
||||
return Quaternion(*(-a for a in self))
|
||||
|
||||
def __truediv__(self, scalar):
|
||||
if isinstance(scalar, Quaternion): # See docs for reason
|
||||
raise ValueError('Cannot divide by Quaternion')
|
||||
return Quaternion(*(a/scalar for a in self))
|
||||
|
||||
def __rtruediv__(self, other):
|
||||
return self.inverse() * other
|
||||
|
||||
# Multiply by quaternion, list, tuple, or scalar: result = self * other
|
||||
def __mul__(self, other):
|
||||
if isinstance(other, Quaternion):
|
||||
w1, x1, y1, z1 = self
|
||||
w2, x2, y2, z2 = other
|
||||
w = w1*w2 - x1*x2 - y1*y2 - z1*z2
|
||||
x = w1*x2 + x1*w2 + y1*z2 - z1*y2
|
||||
y = w1*y2 - x1*z2 + y1*w2 + z1*x2
|
||||
z = w1*z2 + x1*y2 - y1*x2 + z1*w2
|
||||
return Quaternion(w, x, y, z)
|
||||
length = _arglen(other)
|
||||
if length == 0: # Assume other is scalar
|
||||
return Quaternion(*(a * other for a in self))
|
||||
elif length == 3:
|
||||
return Quaternion(0, *(a * b for a, b in zip(self[1:], other)))
|
||||
# length == 4:
|
||||
return Quaternion(*(a * b for a, b in zip(self, other)))
|
||||
|
||||
def __rmul__(self, other):
|
||||
return self * other # Multiplication by scalars and tuples is commutative
|
||||
|
||||
def __add__(self, other):
|
||||
if isinstance(other, Quaternion):
|
||||
return Quaternion(*(a + b for a, b in zip(self, other)))
|
||||
length = _arglen(other)
|
||||
if length == 0: # Assume other is scalar
|
||||
return Quaternion(self[0] + other, *self[1:]) # ? Is adding a scalar meaningful?
|
||||
elif length == 3:
|
||||
return Quaternion(0, *(a + b for a, b in zip(self[1:], other)))
|
||||
# length == 4:
|
||||
return Quaternion(*(a + b for a, b in zip(self, other)))
|
||||
|
||||
def __radd__(self, other):
|
||||
return self.__add__(other)
|
||||
|
||||
def __sub__(self, other):
|
||||
if isinstance(other, Quaternion):
|
||||
return Quaternion(*(a - b for a, b in zip(self, other)))
|
||||
length = _arglen(other)
|
||||
if length == 0: # Assume other is scalar
|
||||
return Quaternion(self[0] - other, *self[1:]) # ? Is this meaningful?
|
||||
elif length == 3:
|
||||
return Quaternion(0, *(a - b for a, b in zip(self[1:], other)))
|
||||
# length == 4:
|
||||
return Quaternion(*(a - b for a, b in zip(self, other)))
|
||||
|
||||
def __rsub__(self, other):
|
||||
return other + self.__neg__() # via __radd__
|
||||
|
||||
def isrot(self):
|
||||
return isclose(abs(self), 1.0, rel_tol = mdelta)
|
||||
|
||||
def isvec(self):
|
||||
return isclose(self[0], 0, abs_tol = adelta)
|
||||
|
||||
def __matmul__(self, rot):
|
||||
return rot * self * rot.conjugate()
|
||||
|
||||
def rrot(self, rot):
|
||||
return rot.conjugate() * self * rot
|
||||
|
||||
# A vector quaternion has real part 0. It can represent a point in space.
|
||||
def Vector(x, y, z):
|
||||
return Quaternion(0, x, y, z)
|
||||
|
||||
Point = Vector
|
||||
|
||||
# A rotation quaternion is a unit quaternion i.e. magnitude == 1
|
||||
def Rotator(theta=0, x=0, y=0, z=0):
|
||||
s = sin(theta/2)
|
||||
m = sqrt(x*x + y*y + z*z) # Convert to unit vector
|
||||
if m > 0:
|
||||
return Quaternion(cos(theta/2), s*x/m, s*y/m, s*z/m)
|
||||
else:
|
||||
return Quaternion(1, 0, 0, 0) # Identity quaternion
|
||||
|
||||
def Euler(heading, pitch, roll):
|
||||
cy = cos(heading * 0.5);
|
||||
sy = sin(heading * 0.5);
|
||||
cp = cos(pitch * 0.5);
|
||||
sp = sin(pitch * 0.5);
|
||||
cr = cos(roll * 0.5);
|
||||
sr = sin(roll * 0.5);
|
||||
|
||||
w = cr * cp * cy + sr * sp * sy;
|
||||
x = sr * cp * cy - cr * sp * sy;
|
||||
y = cr * sp * cy + sr * cp * sy;
|
||||
z = cr * cp * sy - sr * sp * cy;
|
||||
return Quaternion(w, x, y, z) # Tait-Bryan angles but z == towards sky
|
||||
|
|
@ -0,0 +1,201 @@
|
|||
# quat_test.py Test for quat.py
|
||||
|
||||
# Released under the MIT License (MIT). See LICENSE.
|
||||
# Copyright (c) 2020 Peter Hinch
|
||||
|
||||
from math import sin, cos, isclose, pi, sqrt
|
||||
from quat import *
|
||||
|
||||
print('Properties')
|
||||
q1 = Quaternion(1, 2, 3, 4)
|
||||
q1.w = 5
|
||||
q1.x = 6
|
||||
q1.y = 7
|
||||
q1.z = 8
|
||||
assert q1 == Quaternion(5, 6, 7, 8)
|
||||
assert (q1.w, q1.x, q1.y, q1.z) == (5, 6, 7, 8)
|
||||
|
||||
# Numpy demo at https://quaternion.readthedocs.io/en/latest/README.html
|
||||
print('Hamilton product')
|
||||
q1 = Quaternion(1, 2, 3, 4)
|
||||
q2 = Quaternion(5, 6, 7, 8)
|
||||
q3 = Quaternion(-60, 12, 30, 24)
|
||||
assert q3 == q1 * q2
|
||||
|
||||
print('Iterator protocol')
|
||||
assert Quaternion(*q1) == q1
|
||||
assert list(q1[1:]) == [2,3,4]
|
||||
foo = iter(q1)
|
||||
assert next(foo) == 1
|
||||
|
||||
print('Assign from tuple')
|
||||
q1[1:] = (9, 10, 11)
|
||||
assert list(q1[1:]) == [9, 10, 11]
|
||||
q1[:] = (8, 9, 10, 99)
|
||||
assert list(q1[:]) == [8, 9, 10, 99]
|
||||
|
||||
print('Assign from scalar')
|
||||
q1[0] = 88
|
||||
assert list(q1[:]) == [88, 9, 10, 99]
|
||||
|
||||
print('Negation')
|
||||
q1 = Quaternion(1, 2, 3, 4)
|
||||
q2 = Quaternion(-1, -2, -3, -4)
|
||||
assert -q1 == q2
|
||||
|
||||
print('Comparison operators and unary +')
|
||||
assert (q1 is +q1) == False
|
||||
assert q1 == +q1
|
||||
assert (q1 is q1.copy()) == False
|
||||
assert q1 == q1.copy()
|
||||
assert q1 >= q1.copy()
|
||||
assert q1 <= q1.copy()
|
||||
assert (q1 < q1.copy()) == False
|
||||
assert (q1 > q1.copy()) == False
|
||||
|
||||
q2 = Quaternion(1, 2.1, 3, 4)
|
||||
assert q2 > q1
|
||||
assert q1 < q2
|
||||
assert q2 >= q1
|
||||
assert q1 <= q2
|
||||
assert (q1 == q2) == False
|
||||
assert q1 != q2
|
||||
|
||||
print('Scalar add')
|
||||
q2 = Quaternion(5, 2, 3, 4)
|
||||
assert q2 == q1 + 4
|
||||
|
||||
print('Scalar subtract')
|
||||
q2 = Quaternion(-3, 2, 3, 4)
|
||||
assert q2 == q1 - 4
|
||||
|
||||
print('Scalar multiply')
|
||||
q2 = Quaternion(2, 4, 6, 8)
|
||||
assert q2 == q1 * 2
|
||||
|
||||
print('Scalar divide')
|
||||
q2 = Quaternion(0.5, 1, 1.5, 2)
|
||||
assert q2 == q1/2
|
||||
|
||||
print('Conjugate')
|
||||
assert q1.conjugate() == Quaternion(1, -2, -3, -4)
|
||||
|
||||
print('Inverse')
|
||||
assert q1.inverse() * q1 == Quaternion(1, 0, 0, 0)
|
||||
|
||||
print('Multiply by tuple')
|
||||
assert q1*(2,3,4) == Quaternion(0, 4, 9, 16)
|
||||
assert q1*(4,5,6,7) == Quaternion(4, 10, 18, 28)
|
||||
|
||||
print('Add tuple')
|
||||
assert q1 + (2,3,4) == Quaternion(0, 4, 6, 8)
|
||||
assert q1 + (4,5,6,7) == Quaternion(5, 7, 9, 11)
|
||||
|
||||
print('abs(), len(), str()')
|
||||
assert abs(Quaternion(2,2,2,2)) == 4
|
||||
assert len(q1) == 4
|
||||
assert str(q1) == 'w = 1.00 x = 2.00 y = 3.00 z = 4.00'
|
||||
|
||||
print('Rotation')
|
||||
p = Vector(0, 1, 0)
|
||||
r = Rotator(pi/4, 0, 0, 1)
|
||||
rv = p @ r # Anticlockwise about z axis
|
||||
assert isclose(rv.w, 0, abs_tol=mdelta)
|
||||
assert isclose(rv.x, -sin(pi/4), rel_tol=mdelta)
|
||||
assert isclose(rv.y, sin(pi/4), rel_tol=mdelta)
|
||||
assert isclose(rv.z, 0, abs_tol=mdelta)
|
||||
|
||||
|
||||
p = Vector(1, 0, 0)
|
||||
r = Rotator(-pi/4, 0, 0, 1)
|
||||
rv = p @ r # Clockwise about z axis
|
||||
assert isclose(rv.w, 0, abs_tol=mdelta)
|
||||
assert isclose(rv.x, sin(pi/4), rel_tol=mdelta)
|
||||
assert isclose(rv.y, -sin(pi/4), rel_tol=mdelta)
|
||||
assert isclose(rv.z, 0, abs_tol=mdelta)
|
||||
|
||||
p = Vector(0, 1, 0)
|
||||
r = Rotator(-pi/4, 1, 0, 0)
|
||||
rv = p @ r # Clockwise about x axis
|
||||
assert isclose(rv.w, 0, abs_tol=mdelta)
|
||||
assert isclose(rv.x, 0, abs_tol=mdelta)
|
||||
assert isclose(rv.y, sin(pi/4), rel_tol=mdelta)
|
||||
assert isclose(rv.z, -sin(pi/4), rel_tol=mdelta)
|
||||
|
||||
print('Rotation using Euler angles')
|
||||
# Tait-Brian angles DIN9300: I thought z axis is down towards ground.
|
||||
# However https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
|
||||
# and this implementation implies z is towards sky.
|
||||
# https://github.com/moble/quaternion/wiki/Euler-angles-are-horrible
|
||||
|
||||
# Test heading
|
||||
# Yaw/Heading: a +ve value is counter clockwise
|
||||
p = Vector(1, 0, 0) # x is direction of motion
|
||||
r = Euler(pi/4, 0, 0) # Heading 45°.
|
||||
rv = p @ r
|
||||
assert isclose(rv.w, 0, abs_tol=mdelta)
|
||||
assert isclose(rv.x, sin(pi/4), rel_tol=mdelta)
|
||||
assert isclose(rv.y, sin(pi/4), rel_tol=mdelta)
|
||||
assert isclose(rv.z, 0, abs_tol=mdelta)
|
||||
|
||||
# Test pitch
|
||||
# A +ve value is aircraft nose down i.e. z +ve
|
||||
p = Vector(1, 0, 0) # x is direction of motion
|
||||
r = Euler(0, pi/4, 0) # Pitch 45°.
|
||||
rv = p @ r
|
||||
assert isclose(rv.w, 0, abs_tol=mdelta)
|
||||
assert isclose(rv.x, sin(pi/4), rel_tol=mdelta)
|
||||
assert isclose(rv.y, 0, abs_tol=mdelta)
|
||||
assert isclose(rv.z, -sin(pi/4), rel_tol=mdelta) # Implies z is towards sky
|
||||
|
||||
# Test roll
|
||||
# A +ve value is y +ve
|
||||
p = Vector(0, 1, 0) # x is direction of motion. Vector is aircraft wing
|
||||
r = Euler(0, 0, pi/4) # Roll 45°.
|
||||
rv = p @ r
|
||||
assert isclose(rv.w, 0, abs_tol=mdelta)
|
||||
assert isclose(rv.x, 0, abs_tol=mdelta)
|
||||
assert isclose(rv.y, sin(pi/4), rel_tol=mdelta)
|
||||
assert isclose(rv.z, sin(pi/4), rel_tol=mdelta) # Implies z is towards sky
|
||||
|
||||
print('euler() test')
|
||||
r = Euler(pi/4, 0, 0)
|
||||
assert isclose(euler(r)[0], pi/4, rel_tol=mdelta)
|
||||
r = Euler(0, pi/4, 0)
|
||||
assert isclose(euler(r)[1], pi/4, rel_tol=mdelta)
|
||||
r = Euler(0, 0, pi/4)
|
||||
assert isclose(euler(r)[2], pi/4, rel_tol=mdelta)
|
||||
|
||||
print('isrot() and isvec()')
|
||||
assert Quaternion(0, 1, 2, 3).isvec()
|
||||
assert not Quaternion(0, 1, 2, 3).isrot()
|
||||
assert not Quaternion(1, 2, 3, 4).isvec()
|
||||
q = Rotator(1, 1, 1, 1)
|
||||
assert q.isrot()
|
||||
|
||||
print('to_angle_axis()')
|
||||
t = Rotator(1, 1, 1, 1).to_angle_axis()
|
||||
assert isclose(t[0], 1, rel_tol=mdelta)
|
||||
for v in t[1:]:
|
||||
assert isclose(v, sqrt(1/3), rel_tol=mdelta)
|
||||
|
||||
s = '''
|
||||
*** Standard tests PASSED. ***
|
||||
|
||||
The following test of reflected arithmetic operators will fail unless the
|
||||
firmware was compiled with MICROPY_PY_REVERSE_SPECIAL_METHODS.
|
||||
Runs on the Unix build.'''
|
||||
print(s)
|
||||
|
||||
q1 = Quaternion(1, 2, 3, 4)
|
||||
assert 10 + Quaternion(1, 2, 3, 4) == Quaternion(11, 2, 3, 4)
|
||||
assert 1/q1 == q1.inverse()
|
||||
assert 2 * q1 == q1 + q1
|
||||
assert 1 - q1 == -q1 + 1
|
||||
|
||||
s = '''
|
||||
Reverse/reflected operators OK.
|
||||
|
||||
*** All tests PASSED. ***
|
||||
'''
|
||||
print(s)
|
||||
|
|
@ -0,0 +1,54 @@
|
|||
# setup3d.py
|
||||
# Hardware specific setup for 3D demos
|
||||
|
||||
# Released under the MIT License (MIT). See LICENSE.
|
||||
# Copyright (c) 2020 Peter Hinch
|
||||
|
||||
from machine import I2C, SPI, Pin
|
||||
import gc
|
||||
|
||||
# This module must export the following functions
|
||||
# fill(color) Fill the buffer with a color
|
||||
# line(xs, ys, xe, ye, color) Draw a line to the buffer
|
||||
# show() Display result
|
||||
# Also dimension bound variable.
|
||||
|
||||
from ssd1351_16bit import SSD1351 as SSD
|
||||
_HEIGHT = const(128) # SSD1351 variant in use
|
||||
|
||||
# IMU driver for test_imu only. With other IMU's the fusion module
|
||||
# may be used for quaternion output.
|
||||
# https://github.com/micropython-IMU/micropython-fusion
|
||||
from bno055 import BNO055
|
||||
# Initialise IMU
|
||||
i2c = I2C(2)
|
||||
imu = BNO055(i2c)
|
||||
|
||||
# Export color constants
|
||||
WHITE = SSD.rgb(255, 255, 255)
|
||||
GREY = SSD.rgb(100, 100, 100)
|
||||
GREEN = SSD.rgb(0, 255, 0)
|
||||
BLUE = SSD.rgb(0, 0, 255)
|
||||
RED = SSD.rgb(255, 0, 0)
|
||||
YELLOW = SSD.rgb(255, 255, 0)
|
||||
CYAN = SSD.rgb(0, 255, 255)
|
||||
|
||||
|
||||
# Initialise display
|
||||
# Monkey patch size of square viewing area. No. of pixels for a change of 1.0
|
||||
# Viewing area is 128*128
|
||||
DIMENSION = 64
|
||||
|
||||
gc.collect()
|
||||
_pdc = Pin('X1', Pin.OUT_PP, value=0) # Pins are for Pyboard
|
||||
_pcs = Pin('X2', Pin.OUT_PP, value=1)
|
||||
_prst = Pin('X3', Pin.OUT_PP, value=1)
|
||||
_spi = SPI(2) # scl Y9 sda Y10
|
||||
_ssd = SSD(_spi, _pcs, _pdc, _prst, height=_HEIGHT) # Create a display instance
|
||||
|
||||
line = _ssd.line
|
||||
fill = _ssd.fill
|
||||
show = _ssd.show
|
||||
|
||||
def setup():
|
||||
return _ssd
|
||||
|
|
@ -0,0 +1,49 @@
|
|||
# setup3d_lcd160cr.py
|
||||
# Hardware specific setup for 3D demos
|
||||
|
||||
# Released under the MIT License (MIT). See LICENSE.
|
||||
# Copyright (c) 2020 Peter Hinch
|
||||
|
||||
from machine import I2C, SPI, Pin
|
||||
import framebuf
|
||||
import gc
|
||||
from lcd160cr import LCD160CR as SSD
|
||||
from lcd160cr import LANDSCAPE
|
||||
# IMU driver for test_imu only. With other IMU's the fusion module
|
||||
# may be used for quaternion output.
|
||||
# https://github.com/micropython-IMU/micropython-fusion
|
||||
#from bno055 import BNO055
|
||||
## Initialise IMU
|
||||
#i2c = I2C(1)
|
||||
#imu = BNO055(i2c)
|
||||
|
||||
# Export color constants
|
||||
WHITE = SSD.rgb(255, 255, 255)
|
||||
GREY = SSD.rgb(100, 100, 100)
|
||||
GREEN = SSD.rgb(0, 255, 0)
|
||||
BLUE = SSD.rgb(0, 0, 255)
|
||||
RED = SSD.rgb(255, 0, 0)
|
||||
YELLOW = SSD.rgb(255, 255, 0)
|
||||
CYAN = SSD.rgb(0, 255, 255)
|
||||
BLACK = SSD.rgb(0, 0, 0)
|
||||
|
||||
# DIMENSION No. of pixels for a change of 1.0
|
||||
# Viewing area is 128*128
|
||||
DIMENSION = 64
|
||||
|
||||
gc.collect()
|
||||
_buf = bytearray(160*128*2) # 40KiB
|
||||
_fb = framebuf.FrameBuffer(_buf, 160, 128, framebuf.RGB565)
|
||||
_lcd = SSD('Y')
|
||||
_lcd.set_orient(LANDSCAPE)
|
||||
_lcd.set_spi_win(0, 0, 160, 128)
|
||||
# Standard functions
|
||||
line = _fb.line
|
||||
fill = _fb.fill
|
||||
|
||||
def show():
|
||||
gc.collect()
|
||||
_lcd.show_framebuf(_fb)
|
||||
|
||||
def setup():
|
||||
return _lcd
|
||||
|
|
@ -0,0 +1,51 @@
|
|||
# test3d.py 3D objects created using quaternions
|
||||
|
||||
# Released under the MIT License (MIT). See LICENSE.
|
||||
# Copyright (c) 2020 Peter Hinch
|
||||
|
||||
from math import pi
|
||||
import gc
|
||||
from quat import Rotator
|
||||
import graph3d as g3d
|
||||
from setup3d import * # Hardware setup and colors
|
||||
|
||||
# Dict of display objects, each comprises a Shape
|
||||
dobj = g3d.DisplayDict(setup(), pi/6, 5) # Camera angle and z distance
|
||||
|
||||
dobj['axes'] = g3d.Axes(WHITE) # Draw axes
|
||||
|
||||
def demo():
|
||||
dobj['cone'] = g3d.Cone(GREEN) * 0.7
|
||||
# Draw rectangle to check camera perspective
|
||||
square = (g3d.Square(YELLOW) -(0.5, 0.5, 0)) * 1.3
|
||||
rot = Rotator(pi/12, 1, 0, 0)
|
||||
dobj['perspective'] = square
|
||||
for _ in range(24):
|
||||
dobj['perspective'] @= rot
|
||||
dobj.show()
|
||||
rot = Rotator(pi/12, 0, 1, 0)
|
||||
for _ in range(24):
|
||||
dobj['perspective'] @= rot
|
||||
dobj.show()
|
||||
rot = Rotator(pi/12, 0, 0, 1)
|
||||
for _ in range(24):
|
||||
dobj['perspective'] @= rot
|
||||
dobj.show()
|
||||
dobj['perspective'] = g3d.Circle(RED) * 0.7 @ Rotator(pi/24, 0, 0, 1) # (1, 1, 0.5) for ellipse
|
||||
for _ in range(24):
|
||||
dobj['perspective'] @= rot
|
||||
dobj.show()
|
||||
del dobj['cone']
|
||||
del dobj['perspective']
|
||||
gc.collect()
|
||||
print('RAM free {} alloc {}'.format(gc.mem_free(), gc.mem_alloc()))
|
||||
dobj['perspective'] = g3d.Sphere(CYAN) * 0.5 - (0.5, 0.5, 0)
|
||||
rot = Rotator(pi/96, 1, 0, 0)
|
||||
gc.collect()
|
||||
print('RAM free {} alloc {}'.format(gc.mem_free(), gc.mem_alloc()))
|
||||
for _ in range(20):
|
||||
dobj['perspective'] += (0.025, 0.025, 0) # @= rot
|
||||
dobj.show()
|
||||
del dobj['perspective']
|
||||
|
||||
demo()
|
||||
|
|
@ -0,0 +1,38 @@
|
|||
# test_imu.py Demo of cube being rotated by IMU data.
|
||||
|
||||
# Released under the MIT License (MIT). See LICENSE.
|
||||
# Copyright (c) 2020 Peter Hinch
|
||||
|
||||
from math import pi
|
||||
from time import sleep
|
||||
import gc
|
||||
from quat import Rotator, euler
|
||||
import graph3d as g3d
|
||||
from setup3d import * # Hardware setup and colors
|
||||
|
||||
|
||||
def imu_demo():
|
||||
# Dict of display objects, each comprises a Shape
|
||||
dobj = g3d.DisplayDict(setup(), pi/6, 5) # Camera angle and z distance
|
||||
dobj['axes'] = g3d.Axes(WHITE) # Draw axes
|
||||
# Create a reference cube. Don't display.
|
||||
cube = g3d.Cube(RED, BLUE, GREEN) * (0.8, 0.8, 0.8) - (0.4, 0.4, 0.4)
|
||||
imuquat = Rotator()
|
||||
x = 0
|
||||
while True:
|
||||
dobj.show()
|
||||
sleep(0.1)
|
||||
imuquat[:] = imu.quaternion() # Assign IMU data to Quaternion instance
|
||||
# imuquat.normalise() No need: BNo055 data is a rotation quaternion
|
||||
dobj['cube'] = cube @ imuquat
|
||||
x += 1
|
||||
if x == 10:
|
||||
x = 0
|
||||
print('Quat heading {0:5.2f} roll {2:5.2f} pitch {1:5.2f}'.format(*euler(imuquat)))
|
||||
print('IMU heading {:5.2f} roll {:5.2f} pitch {:5.2f}'.format(*[x * pi/180 for x in imu.euler()]))
|
||||
gc.collect()
|
||||
print(gc.mem_free(), gc.mem_alloc())
|
||||
del dobj['cube']
|
||||
|
||||
imu_demo()
|
||||
|
||||
Ładowanie…
Reference in New Issue