New calibration so100

[pkooij]

What this does

Use new simpler calibration method for so100

The raw motor position value always outputs 0...4096

In the old calibration in run_arm_manual_calibration():

    print("\nMove arm to zero position")
    print("See: " + URL_TEMPLATE.format(robot=robot_type, arm=arm_type, position="zero"))
    input("Press Enter to continue...") 

    # We arbitrarily chose our zero target position to be a straight horizontal position with gripper upwards and closed.
    # It is easy to identify and all motors are in a "quarter turn" position. Once calibration is done, this position will
    # correspond to every motor angle being 0. If you set all 0 as Goal Position, the arm will move in this position.
    zero_target_pos = convert_degrees_to_steps(ZERO_POSITION_DEGREE, arm.motor_models)

1. Note that in the comments this is said: this position will correspond to every motor angle being 0. If you set all 0 as Goal Position, the arm will move in this position.. Thus this "laying down" position of the robot is set as "homed" position with all virtual zero's for each joint in this position.

Now we go further

    # Compute homing offset so that `present_position + homing_offset ~= target_position`.
    zero_pos = arm.read("Present_Position")
    homing_offset = zero_target_pos - zero_pos

2. Now here is this zero position being read. zero_target_pos is a list of 0's. Thus the homing offset that is being set is: 0 - zero_pos. zero_pos is the raw motor value from the motor in this position.

    # The rotated target position corresponds to a rotation of a quarter turn from the zero position.
    # This allows to identify the rotation direction of each motor.
    # For instance, if the motor rotates 90 degree, and its value is -90 after applying the homing offset, then we know its rotation direction
    # is inverted. However, for the calibration being successful, we need everyone to follow the same target position.
    # Sometimes, there is only one possible rotation direction. For instance, if the gripper is closed, there is only one direction which
    # corresponds to opening the gripper. When the rotation direction is ambiguous, we arbitrarely rotate clockwise from the point of view
    # of the previous motor in the kinetic chain.
    print("\nMove arm to rotated target position")
    print("See: " + URL_TEMPLATE.format(robot=robot_type, arm=arm_type, position="rotated"))
    input("Press Enter to continue...")

    rotated_target_pos = convert_degrees_to_steps(ROTATED_POSITION_DEGREE, arm.motor_models)

    # Find drive mode by rotating each motor by a quarter of a turn.
    # Drive mode indicates if the motor rotation direction should be inverted (=1) or not (=0).
    rotated_pos = arm.read("Present_Position")
    drive_mode = (rotated_pos < zero_pos).astype(np.int32)

3. We now use our previous zero_pos to calculate the direction of the motors by comparing it with the current read rotated_pos and look if we positively turned all joint 90 degrees of 1024 ticks or negatively to know the drive_mode.
4. We also calulate rotated_target_pos which is a list of 1024 (used below). Which represents this 90 degrees shift. ROTATED_POSITION_DEGREE is set to 90 at the top of the code.

    # Re-compute homing offset to take into account drive mode
    rotated_drived_pos = apply_drive_mode(rotated_pos, drive_mode)
    homing_offset = rotated_target_pos - rotated_drived_pos

5. Now here the homing_offset is being overwritten thus the previous set homing_offset = zero_target_pos - zero_pos wasn't needed for anything. (we only needed the zero_pos to calculate the drive mode (direction))
6. homing_offset = rotated_target_pos - rotated_drived_pos Here the homing offset is calculated taking the list of 1024 (rotated_target_pos) and subtracting the current position (rotated_drived_pos). rotated_drived_pos is just the current position rotated_pos (read in the rotated position) with the right direction (0,1) applied. homing_offset is finally saved to the yaml for later use. And is basically thus the position of the arm in the zero_pos as told here: this position will correspond to every motor angle being 0. If you set all 0 as Goal Position, the arm will move in this position.

We then load in the calibration from the yaml file and use the homing_offset for each joint in the read and write with apply_calibration() and revert_calibration() respectively.

In apply_calibration() called in read():

# Convert from range [-2**31, 2**31[ to
# nominal range ]-resolution, resolution[ (e.g. ]-2048, 2048[)
values[i] += homing_offset

# Convert from range ]-resolution, resolution[ to
# universal float32 centered degree range ]-180, 180[
values[i] = values[i] / (resolution // 2) * HALF_TURN_DEGREE

Thus here we first add the homing_offset, which places the virtual zero (homed position/position for the user) of any raw_value from 0...4096 from the motor, at the zero_pos set earlier.

Example:
If the read position (in the rotated calibration position) was 1000, then this will happen:
rotated_pos = 1000
rotated_target_pos = 1024 This is from a list that is always 1024 (as discussed before)
rotated_drived_pos = apply_drive_mode(rotated_pos, drive_mode) # This will only *-1 if drive mode is 1.
Thus rotated_drived_pos = 1000 (is still 1000)
homing_offset = rotated_target_pos - rotated_drived_pos = 1024 - 1000 = 24

Then in a read() when the motor is in the zero position specified earlier, robot "laying down".
We will read the raw motor position of -24 (Because it is 90 degrees from the position we read when homing (1000) in the rotated position)

Thus in apply_calibration() we do values[i] += homing_offset -> -24 + 24 = 0 And now we have the homed position.

If the read position (in the rotated calibration position) was 1000, then this will happen:
rotated_pos = 3500
rotated_target_pos = 1024
rotated_drived_pos = apply_drive_mode(rotated_pos, drive_mode)
Thus rotated_drived_pos = 3500
homing_offset = rotated_target_pos - rotated_drived_pos = 1024 - 3500 = -2476

Then in a read() when the motor is in the zero position specified earlier, robot "laying down".
We will read the raw motor position of 2476 (Because it is 90 degrees from the position we read when homing (3500) in the rotated position)

Thus in apply_calibration() we do values[i] += homing_offset -> -2476 + 2476 = 0 And now we have the homed position.

This proves that the way you assemble the motor doesn't matter (in the old calibration and thus also not in the new calibration).

What however happens is that because we do a "shift" here: values[i] = values[i] / (resolution // 2) * HALF_TURN_DEGREE in the apply_calibration() Where we re-center into a symmetric range (e.g., [-2048, 2047] if resolutions==4096) and the middle homing position will be virtual 0!

This is not a problem for most joints because they were "homed" in the middle of their actual range. But for joints (2, 3): shoulder_lift and elbow_flex which where "homed" not in the middle but at one end of their (190) degree range, this creates a problem.

The problem is that the rest position is on the other side of their (190) degree range. And when we do values[i] = values[i] / (resolution // 2) * HALF_TURN_DEGREE We "reserve" each of our unique 0...4096 positions (homed) to either -2048...2047. This range directly maps to -180..180 later. But because we "homed" for these two joints at one end of the range when we then let them in their "rest" (normal rest position for the robot) they are at the other side of the range, which is either < -180 or <180. (At the turnoff point). And because we only communicate with the robot (often from this rest position). We don't know how it got there (FeeTechs don't track multi turn). This means internally in our code we can for those joints either start at -180 (or -160..-180) or at (160..180). This causes the problem.

To fix this we can simply also take the actual center of these 2 joints as our homing position. Because then it physically won't reach or cross our "homed" threshold or jump between 180..-180.

Extra:
This does the invert of apply_calibration() when writing to calculate back to the raw motor position.
In revert_calibration() called in write()

# Convert from nominal 0-centered degree range [-180, 180] to
 # 0-centered resolution range (e.g. [-2048, 2048] for resolution=4096)
values[i] = values[i] / HALF_TURN_DEGREE * (resolution // 2)

# Substract the homing offsets to come back to actual motor range of values
# which can be arbitrary.
values[i] -= homing_offset

Conclusion:
It doesn't matter how the motors are assembled. It does matter where we home.

This PR: homes in the middle of all joints. Thus we can get rid of all autocorrect_calibration() and other "complex" code. The range now is always 180...-180. However, this breaks current calibration method, because the calibration positions are different (motor of shoulder_lift and elbow_flex are shifted 90 degrees from original homing (which wasn't in the center), the virtual zero of the robot is now when every motor is in the middle. This influences policies trained with the "old" calibration method and datasets that are already on the hub (could be converted).

Further to discuss: when to merge this? (possibly with another breaking change)

TODO:

• Add single motor calibration and store + add new assembly instructions for integrated calibration and assembly (see todo in code)
• Add new calibration instructions to tutorial of SO100 and MobileSO100 (see todo in code)

Note: the Feetech can't reliably do multi turn. Because when the motor goes beyond 180, even though we tell it to move again to 10. It will try to go there the other way around.

How it was tested

• Using calibration_visualization.py script (comparing old and new calibration to ensure they are the same in the end for the policy)
• On real robot do teleoperation
• Look at the visualize dataset on the hub

[imstevenpmwork]

Hello @pkooij, thanks for the PR! Is this still relevant?

[aliberts]

Looks very nice but I'm not yet sure if it works. I've tried it on this branch (which is my refactor PR + your changes) and I'm having calibration issues (like some motors go all the way around in the other direction past a certain point). There's also the (strong) probability that I did something wrong along the way, let's discuss this IRL with the robot ;)