Tilt sensor and acceleration

[Debenben]

In my spare-time I am building a lego robot dog and would like to teach it to walk. Each leg has a technic-hub in the upper part and a (WeDo-)tilt-sensor in the lower part to keep track of the position using acceleration. For this I am using the tilt-sensor in "CAL" mode where it reads 3 angles that are supposed to go from -45 to 45 degrees. If you shake the sensors you can easily get values above or below that interval. For that reason and to be similar to the integrated accelerometer in the technic-hub I convert the values to acceleration like this:

from pybricks.iodevices import PUPDevice
from pybricks.parameters import Port
from pybricks.tools import wait
from umath import pi, sin, cos, sqrt

# Initialize a TiltSensor as a generic PUPDevice on Port A
tiltSensor = PUPDevice(Port.A)

while(True):
    # Read the values of the sensor in mode 3 which is CAL mode
    t = tiltSensor.read(3)

    # Convert angles in degree to radiant
    t = [v*pi/180 for v in t]

    # Calculate acceleration vector from tilt angles
    acceleration = [sin(t[0])*cos(t[1])*cos(t[2]),
                    cos(t[0])*sin(t[1])*cos(t[2]),
                    cos(t[0])*cos(t[1])*sin(t[2])]
    
    # Normalize acceleration vector such that it has
    # a magnitude of 1 at rest in standard gravity
    acceleration = [v*sqrt(2) for v in acceleration]

    print(acceleration)
    wait(100)

What do you think of this? This could maybe be a nice addition to the library. Maybe as an example in the documentation of the tilt-sensor although it is not using the tilt-sensor-class? Or the "CAL" mode could be implemented and then used like this? I also made a proof of concept of implementing this conversion itself

https://github.com/pybricks/pybricks-micropython/compare/master...Debenben:pybricks-micropython:acceleration?expand=1

I did not try to optimize it for compilation size, but I imagine one could make it smaller by restructuring and possibly reusing code.

Also I wanted to format the acceleration output like that of the internal technic-hub accelerometer but it seems the code does not match the documentation. The magnitude of the vector my technic-hub returns with the current master branch of pybricks-micropython is around 1000. The documentation

https://docs.pybricks.com/en/latest/hubs/technichub.html#pybricks.hubs.TechnicHub.imu.acceleration

sais the unit is mm/s^2. Gravitational acceleration on earth is around 9.81m/s^2 = 9810 mm/s^2 which is about 10 times larger than 1000 from the technic-hub.

[laurensvalk]

I am glad you asked! I have been wondering about exactly this, and prepared some code as well (I might need to dig it up somewhere).

I haven't added it to the device class because I wasn't sure anyone else would ever need it 😄

The use case I was originally after, was to provide tilt in the full range (outside of -45/45), and optionally also outside the default orientation as we do allow for hubs.

What do you think of this? This could maybe be a nice addition to the library. Maybe as an example in the documentation of the tilt-sensor although it is not using the tilt-sensor-class?

We probably couldn't add it to the Move Hub firmware due to build size, and it would be nice to keep the classes the same on all hubs.

So having a few examples like those might be the best thing to start with indeed. It would be great if you want to make a pull request in the docs.

Here's another possibility if you want to take it further. Perhaps as an additional example: Now that we support importing code across modules in Pybricks Code, it's possible to extend the TiltSensor class with an extra method.

In your example:

    # Calculate acceleration vector from tilt angles
    acceleration = [sin(t[0])*cos(t[1])*cos(t[2]),
                    cos(t[0])*sin(t[1])*cos(t[2]),
                    cos(t[0])*cos(t[1])*sin(t[2])]

Does this work along the whole sphere of rotations? Looking back at my notes, I found that I had to use various if/else branches to ensure that I didn't use one of the values that was capped at 45 degrees, but always the other two to get tilt/acceleration.

[Debenben]

For my initial goal I did not really need an accurate magnitude but then it started bothering me, so instead of the constant $\sqrt2$ I came up with this:

    # Define shortcut notation for calculation of normalization factor
    a = cos(t[0])*cos(t[0])
    b = cos(t[1])*cos(t[1])
    c = cos(t[2])*cos(t[2])

    # Normalize acceleration vector such that it has
    # a magnitude of 1 at rest in standard gravity
    factor = sqrt(6/(2*a + 2*b + 2*c - a*b - a*c - b*c))
    acceleration = [v*factor for v in acceleration]

The idea behind this formula is normalizing all cases where one angle is capped at 45 degrees and averaging over it.

Replacing $\sin^2\alpha=1-\cos^2\alpha$ and applying the shortcut notation from above the squared magnitude of the acceleration vector is $$(1-a)bc+a(1-b)c+ab(1-c)$$ The cases of angles at 45 degrees are $a=\frac12$, $b=\frac12$ or $c=\frac12$ with squared magnitudes $$\frac12b+\frac12c-\frac12bc$$ $$\frac12a+\frac12c-\frac12ac$$ $$\frac12a+\frac12b-\frac12ab$$
We add up the three terms and divide by 3. After taking the square-root again the result is the magnitude which we divide our acceleration with to normalize it.

With this the magnitudes look better, but the real proof or disprove would be a proper experiment. Also, I would be curious to find out what happens in a centrifuge: Does the sensor adjust to the increased acceleration or not? I don't have any proper equipment for that but if I find some time again I'll try to improvise.

[laurensvalk]

As far as I know, the values are capped in software at 45 degrees, which makes me think that there isn't a single continuous mapping from the call values to angles or acceleration.

Looking at my notes, I think there was always one value at or close to +/-45 degrees, so I had 6 simple variants of the following:

For example, if the third value v3 was 45 then the direction of the acceleration vector is:

And then 5 other variants like this, depending on which value was saturated.

[laurensvalk]

I found this code snippet in my notes and just tested it again.

Note: this snippet does all steps numerically so we can see what is going on, but each of the 6 cases can be reduced symbolically to a very short form as given in the example above.

from pybricks.parameters import Port
from pybricks.iodevices import PUPDevice
from pybricks.tools import wait

from umath import pi, cos, sin

def prod(A, B):
    return [[sum([A[i][c]*B[c][j] for c in range(3)]) for j in range(3)] for i in range(3)]


class TiltSensor(PUPDevice):

    def __init__(self, port):
        self.io = PUPDevice(port)
        self.io.read(3)

    def acceleration(self):
        v1, v2, v3 = self.read(3)
        av1 = abs(v1)
        av2 = abs(v2)
        av3 = abs(v3)
        if av1 != 45 and av2 != 45 and av3 != 45:
            # did not find 45, so make the maximum 45 anway
            if av1 >= av2 and av1 >= av3:
                v1 = 45 if v1 > 0 else -45
            if av2 >= av1 and av2 >= av3:
                v2 = 45 if v2 > 0 else -45
            else:
                v3 = 45 if v3 > 0 else -45

        # In the following 2 cases, the front side points forwards
        if v3 == 45:
            # upwards=top
            a = -v2/180*pi
            b = v1/180*pi
            # print('top   :', a, b)
            pre = [[ 1, 0, 0],
                   [ 0, 1, 0],
                   [ 0, 0, 1]]
            R_w_s = [
                [ cos(b),            0,           sin(b)],
                [ sin(a)*sin(b), cos(a), -sin(a)*cos(b)],
                [-sin(b)*cos(a), sin(a),  cos(a)*cos(b)]
            ]
        elif v3 == -45:
            # upwards=bottom
            a = -v2/180*pi
            b = -v1/180*pi
            # print('bottom:', a, b)
            pre = [[-1, 0, 0],
                   [ 0, 1, 0],
                   [ 0, 0,-1]]
            R_w_s = [[cos(b), 0, sin(b)], [-sin(a)*sin(b), cos(a), sin(a)*cos(b)], [-sin(b)*cos(a), -sin(a), cos(a)*cos(b)]]
        # In the following 4 cases, the top side points forwards
        elif v1 == 45:
            # upward=left
            a = v3/180*pi
            b = -v2/180*pi
            # print('left  :', a, b)
            pre = [[ 0,-1, 0],
                   [ 0, 0, 1],
                   [-1, 0, 0]]
            R_w_s = [[cos(a)*cos(b), -sin(b)*cos(a), -sin(a)], [sin(b), cos(b), 0], [sin(a)*cos(b), -sin(a)*sin(b), cos(a)]]
        elif v1 == -45:
            # upward=right
            a = v3/180*pi
            b = v2/180*pi
            # print('right :', a, b)
            pre = [[ 0, 1, 0],
                   [ 0, 0, 1],
                   [ 1, 0, 0]]
            R_w_s = [[cos(a)*cos(b), -sin(b)*cos(a), sin(a)], [sin(b), cos(b), 0], [-sin(a)*cos(b), sin(a)*sin(b), cos(a)]]
        elif v2 == 45:
            # upward=back
            a = v3/180*pi
            b = v1/180*pi
            # print('back  :', a, b)
            pre = [[ 1, 0, 0],
                   [ 0, 0, 1],
                   [ 0,-1, 0]]
            R_w_s = [[cos(b), -sin(b), 0], [sin(b)*cos(a), cos(a)*cos(b), -sin(a)], [sin(a)*sin(b), sin(a)*cos(b), cos(a)]]
        elif v2 == -45:
            # upward=front
            a = v3/180*pi
            b = -v1/180*pi
            # print('front :', a, b)
            pre = [[-1, 0, 0],
                   [ 0, 0, 1],
                   [ 0, 1, 0]]
            R_w_s = [[cos(b), -sin(b), 0], [sin(b)*cos(a), cos(a)*cos(b), sin(a)], [-sin(a)*sin(b), -sin(a)*cos(b), cos(a)]]

        pr = prod(pre, R_w_s)
        # for row in pr:
        #     print([round(e, 2) for e in row])
        # print("-------------")

        return tuple(round(pr[2][i]*9810) for i in range(3))


sensor = TiltSensor(Port.A)

while True:

    print(sensor.acceleration())
    wait(100)

[Debenben]

@laurensvalk Thanks for sharing the code and the constructive feedback. You are using a more standard definition of spherical angles that would indeed fit with the assumption of one value always being at $\pm 45$ degrees. However this is not what I observe with my sensor.

For an acceleration direction $(1, 1, 0)/\sqrt 2$ two angles are $\pm 45$ degrees and the remaining one 0. This I can observe with my sensor and both your and my angle mapping would predict the same (modulo sign convention and my normalization mess).

An acceleration $(1, 1, 1)/\sqrt 3$ would be mapped to two angles at $\pm 45$ degrees and one at $\pm\arcsin(1/\sqrt 3)\approx 35$ degrees in your model. In my model it would be mapped to all three angles at $\pm\arcsin(1/\sqrt 3) \approx 35$ degrees. With my sensor I get all angles around 35 degrees in this case.

[Debenben]

Quick update: I still believe the tilt angles are to some rescaled "Coordinate Direction Angles" rather than traditional spherical angles, but not sure about the exact definition. I tried to find out by fixing the tiltsensor to a hub and comparing its readings to that of the hub accelerometer. To improve the accuracy of the hub accelerometer I made a simple calibration routine, adding an offset to each axis.

I compared my angle definition above, euler angles and a "small angle approximation" where the "tilt angles" are simply the components of the acceleration vector. All of those and some other approaches perform reasonably well, yet I was not able to reverse-engineer and find an algorithm with numerical approximation and calibration routine that would match completely.

When I saw pybricks/pybricks-micropython#135 I thought people might be interested in my accelerometer calibration. My goal is to use pybricks eventually, currently it is official lego firmware with nodejs: https://github.com/Debenben/babyele/blob/master/src/calibrator.ts. With a library for the least-squares fit, I could rewrite it in C or micropython for use on the hub and it would no longer be limited by bluetooth transfer rate. As far as I know, most accelerometer calibration routines also compute a gain factor and one could allow any three stable positions, the 6 directions are a nice to have but not required.