[Feature] User IMU calibration implementation #1907
Comments
laurensvalk
added
enhancement
laurensvalk
changed the title
|
To see the "before" and "after" situation, try running the following script before and after doing the calibration routine: from pybricks.hubs import ThisHub
from pybricks.pupdevices import Motor, ColorSensor, UltrasonicSensor
from pybricks.parameters import Button, Color, Direction, Port, Side, Stop, Axis
from pybricks.robotics import DriveBase
from pybricks.tools import wait, StopWatch, vector
from umath import acos, degrees, pi, sqrt
hub = ThisHub()
startx = hub.imu.rotation(Axis.X)
starty = hub.imu.rotation(Axis.Y)
startz = hub.imu.rotation(Axis.Z)
while True:
if hub.buttons.pressed():
startx = hub.imu.rotation(Axis.X)
starty = hub.imu.rotation(Axis.Y)
startz = hub.imu.rotation(Axis.Z)
while hub.buttons.pressed():
wait(10)
x = hub.imu.rotation(Axis.X) - startx
y = hub.imu.rotation(Axis.Y) - starty
z = hub.imu.rotation(Axis.Z) - startz
print(f"{x:10.3f}", f"{y:10.3f}", f"{z:10.3f}", f"{hub.imu.heading():10.3f}")
wait(100)You might find that before, a flat rotation around each of the axis is not a perfect 360-degree turn. After calibration, it should be. In one of my hubs, this corrects a y-axis rotation from 354 to 360, which is quite a win. |
|
Before calibration: ~364.something and after 360.9. Great. Was a bit tricky to see what side of the robot inventor has the speaker due to the lighting this time of day. Quite doable calibration scenario. 'naughty ON:' Curious how you will suggest to do the Technic hub 😜 'naughty OFF' Even if a beam is added at the TechnicHub sides it wobbles on the small ridges on the sides. (not LEGO like) Tried the Technichub anyway (ignore if I am out of line) see picture below. All goes well until calibration of the Z side: The z is just a small bit over 0.05. testhub: Great step forwards, thank you! Bert |
|
Stubborn as I am, got a Technic hub calibrated. I have set the z allowance from 0.05 to 0.06 and used this support for the technichub with 3L axle and "connectors number 1" with half pins in them: [EDIT] These settings that are automatically retrieved and applied, might make debugging later a bit harder. And maybe a program to print the settings to re-load after a firmware change. |
|
The routine is now built into the latest firmware. I have updated the testing instructions. API updates are to follow... |
|
Technic hub: Well sure enough the Technic hub has no speaker 😄 |
|
Fixed now. Nightly firmware has been updated. |
|
Technic hub at (Primehub is in a model and the RI-hub still is in "LEGO-mode") |
|
I'm having issues. Could someone help me please? I have tried multiple times. |
|
Thanks for reporting! We should be able to update the test files in a few days. |
I studied the code a bit, I can't figure out the error right now. Possibly lack of knowledge in python. Anyway... I'm using the pybrciks code online editor now. Is there a handy way that I can help find the error? E.g. IDE with step through debugging option? |
|
It seems that the calibration passes but it does not succeed in saving it. It does some value checks to avoid saving bad values. For example, it will reject gyro corrections of more than 10 degrees as this might be a user error. But maybe on some hubs the errors are truly 11 degrees, for example. So could you try running the following adapted script? You can just run it from the online editor. It is the same as before but now it will print the values you found before trying to save it. It should give the same error, but then we have an indication as to why it might be. And then we can relax the tolerances. Click to see adapted script.from pybricks.hubs import ThisHub
from pybricks.parameters import Side, Axis
from pybricks.tools import wait, vector
hub = ThisHub()
def beep(freq):
try:
hub.speaker.beep(freq, 100)
except AttributeError:
# Technic hub does not have a speaker.
pass
wait(10)
def wait_for_stationary(side):
while not hub.imu.stationary() or hub.imu.up() != side:
wait(10)
up_sides = {
Side.FRONT: (0, 0),
Side.BACK: (1, 0),
Side.LEFT: (2, 1),
Side.RIGHT: (3, 1),
Side.TOP: (4, 2),
Side.BOTTOM: (5, 2),
}
gravity = [0] * 6
bias = vector(0, 0, 0)
STATIONARY_COUNT = 1000
def roll_over_axis(axis, new_side):
global bias, bias_count
print("Roll it towards you, without lifting the hub up!")
angle_start = hub.imu.rotation(axis, calibrated=False)
while hub.imu.up() != new_side or not hub.imu.stationary():
_, _, z = hub.imu.orientation() * axis
if abs(z) > 0.07:
print(hub.imu.orientation() * axis)
raise RuntimeError("Lifted it!")
wait(100)
uncalibrated_90_deg_rotation = abs(hub.imu.rotation(axis, calibrated=False) - angle_start)
if abs(uncalibrated_90_deg_rotation - 90) > 10:
raise RuntimeError("Not 90 deg!")
print("Calibrating...")
beep(1000)
rotation_start = vector(
hub.imu.rotation(Axis.X, calibrated=False),
hub.imu.rotation(Axis.Y, calibrated=False),
hub.imu.rotation(Axis.Z, calibrated=False),
)
acceleration = vector(0, 0, 0)
for i in range(STATIONARY_COUNT):
acceleration += hub.imu.acceleration(calibrated=False)
bias += hub.imu.angular_velocity(calibrated=False)
wait(1)
acceleration /= STATIONARY_COUNT
rotation_end = vector(
hub.imu.rotation(Axis.X, calibrated=False),
hub.imu.rotation(Axis.Y, calibrated=False),
hub.imu.rotation(Axis.Z, calibrated=False),
)
if abs(rotation_end - rotation_start) > 1:
raise RuntimeError("Moved it!")
side_index, axis_index = up_sides[new_side]
# Store the gravity value for the current side being up.
# We will visit each side several times. We'll divide by the number
# of visits later.
gravity[side_index] += acceleration[axis_index]
beep(500)
return uncalibrated_90_deg_rotation
calibrate_x = """
Going to calibrate X now!
- Put the hub on the table in front of you.
- top side (display) facing up
- right side (ports BDF) towards you.
"""
calibrate_y = """
Going to calibrate Y now
- Put the hub on the table in front of you.
- top side (display) facing up
- back side (speaker) towards you.
"""
calibrate_z = """
Going to calibrate Z now!
- Put the hub on the table in front of you.
- front side (USB port) facing up
- left side (ports ACE) towards you
"""
REPEAT = 2
# For each 3-axis run, we will visit each side twice.
SIDE_COUNT = REPEAT * 2
def roll_hub(axis, message, start_side, sides):
print(message)
wait_for_stationary(start_side)
beep(500)
rotation = 0
for _ in range(REPEAT):
for side in sides:
rotation += roll_over_axis(axis, side)
return rotation / REPEAT
rotation_x = roll_hub(
Axis.X, calibrate_x, Side.TOP, [Side.LEFT, Side.BOTTOM, Side.RIGHT, Side.TOP]
)
rotation_y = roll_hub(
Axis.Y, calibrate_y, Side.TOP, [Side.FRONT, Side.BOTTOM, Side.BACK, Side.TOP]
)
rotation_z = roll_hub(
Axis.Z, calibrate_z, Side.FRONT, [Side.RIGHT, Side.BACK, Side.LEFT, Side.FRONT]
)
print("angular_velocity_bias=", tuple(bias / SIDE_COUNT / STATIONARY_COUNT / 6))
print("angular_velocity_scale=", (rotation_x, rotation_y, rotation_z))
print("acceleration_correction=", [g / SIDE_COUNT for g in gravity])
hub.imu.settings(
angular_velocity_bias=tuple(bias / SIDE_COUNT / STATIONARY_COUNT / 6),
angular_velocity_scale=(rotation_x, rotation_y, rotation_z),
acceleration_correction=[g / SIDE_COUNT for g in gravity],
)
print("Result: ", hub.imu.settings()) |
|
See below, I cut out the obvious. When I look at the results and look at the code. It looks like something with the acceleration_correction is not within limits. I have installed firmware: primehub-firmware-build-3643-gitcc5521fc.zip |
|
Thank you, this is very helpful! Your hub is reporting the following amount of degrees for each full x, y, z rotation respectively: This is further off the ideal 360 value than I have seen in any of our hubs. We had set the limits at 350-370, so this explains the issue: yours is just outside it. We will increase the tolerances since this is clearly necessary, so that calibration will pass on your hub too. When we do that and you complete this procedure again, you should get a massive improvement for measuring rotation around the Y axis for your hub! |
Thanks for explaining this. Could you give an indication when you have new firmware as a beta? Furthermore I am curious when this feature will officially come as a release. I started reading the C code and I played with: PS when this is working I will also share my results of #1962 apparently i have a hub that has a bit more deviation than tested so far. Maybe this can help. |
Sanity checks help prevent errors, but users have reported valid larger deviations than previously known. See pybricks/support#1907
|
Just increased the tolerances from 10 to 15 degrees, which should cover your hub. You can follow the original instructions to get the new, latest build. Thank you for trying this and reporting this. Now we could fix the tolerances before releasing it! |
|
I have seen a mega improvement. THANKS! Before: Simple test Would It help if I would share the same measurement results as on #1962 (comment) |
We are introducing a builtin routine to calibrate the IMU on all three axes. The user is instructed to place the hub in a particular orientation, and then to rotate the hub towards the user several times. This is repeated for each axis.
The instructions are currently text-only. Ultimately, this should be paired with a visual animation from the Pybricks Code IDE.
To try it out (updated 2024-12-03)
import _imu_calibrateand follow the instructions.To see the results afterwards, do:
The values remain on the hub until you install a new firmware. You can then run the calibration again, or simply restore the values you printed above.
Hub specifics
The SPIKE Prime Hub casing is close enough to a rectangular box. You can just rotate it in place, as in the video below.
The Technic Hub sides aren't really flat enough. You can add some Technic elements like Bert did below.
Video
This video is slightly outdated now, but it shows the gist of the procedure.
calibration2.mp4
Original post
Click to see original post
The technical aspects of calibration are being discussed in #943, this issue is more about the user routine: How can we make it easy for the user to calibrate their gyro? This is an alternative to #1678 to be ready for 3D. It is a bit simpler too since you don't need an external reference.
This would have to be done just once and the results would be saved persistently on the hub. You'd have to do it again if you updated the firmware. This will calibrate the accelerometer and gyro all in one go.
The procedure is as follows, three times. It takes a bit of practice, but it is easy to do.
We could animate this visually in a Pybricks Code pane. This is roughly what the user would have to do:
calibration2.mp4
(Video is cut off. This repeats one more time for the final axis.)
Instructions
The text was updated successfully, but these errors were encountered: