Thoughts about calibration and fixed starting position

[thomasbrus]

Let's say I am building a robot arm. I may use a worm gear to reduce the gear ratio and to make sure the weight of the arm doesn't put load on the motor in stationary position.

At the start of my program, I would like the arm to start in a fixed/known position. For two reasons:

• To know how far I can rotate back and forth before hitting a limit
• If I know the position of the arm at the start, I can apply inverse kinematics to move the robot arm to an absolute x, y, z coordinate.

The usual approach is to drive the motor back and forth using run_until_stalled, that works well for calibrating the steering of a car for example. But considering the weight of the arm and that will be using a worm gear, that is not really an option.

The angular motors also know their zero position, but that is only useful when the gear ratio is <1. If the motor needs to rotate many times (when using a worm gear for example), then resetting it to zero position doesn't mean the arm is in the zero position as well (not necessarily).

Are there any other calibration techniques that I haven't thought about? I suppose I could use touch sensors, but that solution has downsides as well. For now I just settle on manually putting the arm in the right position and returning to the initial position at the end of the program, or when it crashes.

[Debenben]

An idea could be to use acceleration sensors to get the angle with respect to gravity. I am using the differences in acceleration of technichubs and (WeDo-) tilt sensors (using device.read(3)) to calculate leg positions of my lego robot dog. The dog is not finished but getting joint angles this way works.

[thomasbrus]

Nice, I'm also working on a quadruped, same calibration problem there. Can you explain it in a bit more detail? How can you determine the initial angles exactly using the acceleration?

[Debenben]

It looks like this (sorry for bad image quality, currently that is all I have):

Each leg has 3 joints with 3 motors. Two motors are connected to the technic hubs in the body. The third is connected to a technic hub in the upper part of the leg. In the lower part of the leg there is a wedo-tilt sensor. For easier maintenance it is possible to remove the large black gear on the outside and move it by hand.

I have put the code on github (sorry also for bad code quality, I will clean it up once it is finished)

In general it is using the "MotorAngle" values of the motor encoder and can convert it to the angles of the joints and back:
https://github.com/Debenben/babyele/blob/1dce007d2da70a9035fc01004ee20370a786476b/src/conversions.ts#L26
https://github.com/Debenben/babyele/blob/1dce007d2da70a9035fc01004ee20370a786476b/src/conversions.ts#L36

The acceleration is used for calibration after start, validating and recalibrating due to mechanical play of gears. The conversion function is this:
https://github.com/Debenben/babyele/blob/1dce007d2da70a9035fc01004ee20370a786476b/src/conversions.ts#L140
It is working well in "normal" positions. In case a rotation axis is parallel to gravity it does not work. There the angle would be the difference in absolute heading direction which you cannot measure with accelerometers. The values get more and more inaccurate the closer you get to that position. I simply refrain from recalibrating in such a situation and use motor encoders only.

The knee took a lot of redesigns to be able to handle the weight and deal with the cables running through it but it works well now. I did not want to use linear actuators there, but they are definitely the easier option when dealing with larger weights. The top joint with the possibility to infinitely rotate sometimes skips gears under load, that I still would like to redesign.

Another problem is communication with the hubs. I tried conneting them with the lego firmware to one computer with a bluetooth dongle, but I only have bluetooth dongles which refuse to connect to 6 hubs at the same time or they allow to connect 6 but then randomly disconnect a hub when trying to send moderate amounts of messages. With pybricks I can use the observe and broadcast which does not have this problem, but unfortunately for technic hubs there is still a bug where they sometimes randomly get stuck when broadcasting and observing at the same time.

[thomasbrus]

Looks really cool. I understand the part about converting between motor angles and leg positions (kinematics). Not really sure what is going on here though 🤔 how does this accomplish getting the legs in the right position from an unknown starting position?

[Debenben]

The acceleration gets you pan and tilt angle with respect to gravity. Lets assume I measure some acceleration values x, y, and z in the hub in the body of the dog and then the same values in the hub in the upper part of a leg. Then the hub which is in the leg must have the same orientation to gravity. In my case the hubs in the legs are mounted with the light pointing down and facing to the outside, therfore I know if x, y, and z of the hub in body and upper part of the leg are the same, my top part of the leg must be pointing forward at 90 degrees and tilted to the side by 90 degrees.

If I know my gear ratio to be 100:1, the position of both my motors at the top and must be 9000 degrees. Then I would set them to 9000 and request them to move to 0 if I want them to move to my "starting position".

The same you can do comparing acceleration in upper and lower part of the leg. In this case I have one degree of freedom the joint so it is overdetermined and I would have the possibility to check that the legs do not bend where they should not. In my case the devices are mounted such that the tilt angles they measure must always be the same. Therefore I can directly subtract the two pan angles for top and bottom hub which I get with arctan2(z, x) from the acceleration in z and x direction.

[thomasbrus]

Okay I get how you can compare the pan and tilt between the hub in the body and the leg to determine the leg position and thus the motor angles, that makes sense yes 👍 you can also connect the hubs via the Bluetooth Web API by the way, in case you're interested in that I have some code over here.

[BertLindeman]

Thomas,

I have it not at hand now, But I had a tic-tac-toe robot, where I had a smooth calibration function by lowering the torque.
At the moment no time to look for it. So later......

Bert

[laurensvalk]

This is a partial answer, but it is simple and practical. It does not require any extra sensors. I had the same challenge for this 6-legged robot.

When you initialize the motor, you have the option of reset_angle=False, so it will maintain the angle.

So then you can manually initialize once - for example the first time after you boot - and use reset_angle(0) or with some sensible value. And then you don't need calibration again while the hub is on. And you can restart your program many times or change your program, and still maintain the position.

In this robot, I made it choose reset_angle=True if I held one of the buttons when the program was started, and False otherwise.

[thomasbrus]

Sweet! So for how long does it "remember" the angle? How does that work exactly? Apparently it works even when you upload a new version of the program? Thanks, gonna give that a try. To calibrate it once manually is perfectly fine.

[laurensvalk]

The motors always keep counting, even when the program ends. By choosing reset_angle=False, it just doesn't "modulo" it back to the -180 to 180 range.

So the only logic you'd need to add is something like:

should_reset = some_condition_like_left_button_pressed

if should_reset:
   beep
   # here you can manually put the legs in the right place
   wait_for_some_other_condition_so_you_can_indicate_completion
   double beep

motor = Motor(Port.A, reset_angle=should_reset)

[thomasbrus]

Gotcha. Should be fairly easy to do with a button press on an xbox controller for example 😄

[thomasbrus]

Ended up making a separate script for motor calibration

# calibrate.py

from pybricks.hubs import TechnicHub
from pybricks.parameters import Port, Button, Color
from motors import MaybeMotor, NoneMotor
from pybricks.iodevices import XboxController

hub = TechnicHub()
hub.light.on(Color.RED)

controller = XboxController()

hub.light.off()
controller.rumble(50)


def set_corresponding_light(index):
    if index == 0:
        hub.light.on(Color.GREEN)
    elif index == 1:
        hub.light.on(Color.RED)
    elif index == 2:
        hub.light.on(Color.BLUE)
    elif index == 3:
        hub.light.on(Color.YELLOW)


motor_a = MaybeMotor(Port.A)
motor_b = MaybeMotor(Port.B)
motor_c = MaybeMotor(Port.C)
motor_d = MaybeMotor(Port.D)
motors = [motor_a, motor_b, motor_c, motor_d]

index = 0

while index < len(motors) and isinstance(motors[index], NoneMotor):
    index += 1

set_corresponding_light(index)

while True:
    pressed_buttons = controller.buttons.pressed()

    if Button.GUIDE in pressed_buttons:
        for motor in motors:
            motor.reset_angle(0)
        hub.system.shutdown()

    left, right = controller.triggers()
    motors[index].dc(right - left)

    if Button.A in pressed_buttons:
        index = 0
        set_corresponding_light(index)

    if Button.B in pressed_buttons:
        index = 1
        set_corresponding_light(index)

    if Button.X in pressed_buttons:
        index = 2
        set_corresponding_light(index)

    if Button.Y in pressed_buttons:
        index = 3
        set_corresponding_light(index)

# See https://pybricks.com/project/disable-motor-detect/
# motors.py

from pybricks.pupdevices import Motor
from pybricks.parameters import Direction


# A function or method that just does nothing.
def do_nothing(*args, **kwargs):
    return 0


# A class that does nothing and allows any method calls.
class NoneMotor:
    def __getattr__(self, name):
        return do_nothing


# Helper function that creates a Motor-like object, but
# suppresses exceptions about motors not being connected.
def MaybeMotor(port, direction=Direction.CLOCKWISE):
    try:
        # Give a normal motor object if such device is connected.
        return Motor(port, direction)
    except OSError:
        return NoneMotor()

[BertLindeman]

I see more interesting answers here.

So I just post a link to my tic-tac-toe program on bricksafe

Bert

[thomasbrus]

Thanks for sharing the code. I have gone down a similar path in another project (although it was much simpler than your code), but always felt there had to be a more elegant way that doesn't stress the gears and motors as much. That being said it is nice that the calibration is entirely autonomous!