diff --git a/lerobot/common/motors/dynamixel/dynamixel_calibration.py b/lerobot/common/motors/dynamixel/dynamixel_calibration.py deleted file mode 100644 index 94151af2..00000000 --- a/lerobot/common/motors/dynamixel/dynamixel_calibration.py +++ /dev/null @@ -1,152 +0,0 @@ -# Copyright 2024 The HuggingFace Inc. team. All rights reserved. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Logic to calibrate a robot arm built with dynamixel motors""" -# TODO(rcadene, aliberts): move this logic into the robot code when refactoring - -import numpy as np - -from ..motors_bus import MotorNormMode, MotorsBus -from .dynamixel import TorqueMode -from .tables import MODEL_RESOLUTION - -URL_TEMPLATE = ( - "https://raw.githubusercontent.com/huggingface/lerobot/main/media/{robot}/{arm}_{position}.webp" -) - -# The following positions are provided in nominal degree range ]-180, +180[ -# For more info on these constants, see comments in the code where they get used. -ZERO_POSITION_DEGREE = 0 -ROTATED_POSITION_DEGREE = 90 - - -def assert_drive_mode(drive_mode): - # `drive_mode` is in [0,1] with 0 means original rotation direction for the motor, and 1 means inverted. - if not np.all(np.isin(drive_mode, [0, 1])): - raise ValueError(f"`drive_mode` contains values other than 0 or 1: ({drive_mode})") - - -def apply_drive_mode(position, drive_mode): - assert_drive_mode(drive_mode) - # Convert `drive_mode` from [0, 1] with 0 indicates original rotation direction and 1 inverted, - # to [-1, 1] with 1 indicates original rotation direction and -1 inverted. - signed_drive_mode = -(drive_mode * 2 - 1) - position *= signed_drive_mode - return position - - -def convert_degrees_to_steps(degrees: float | np.ndarray, models: str | list[str]) -> np.ndarray: - """This function converts the degree range to the step range for indicating motors rotation. - It assumes a motor achieves a full rotation by going from -180 degree position to +180. - The motor resolution (e.g. 4096) corresponds to the number of steps needed to achieve a full rotation. - """ - resolutions = [MODEL_RESOLUTION[model] for model in models] - steps = degrees / 180 * np.array(resolutions) / 2 - steps = steps.astype(int) - return steps - - -def compute_nearest_rounded_position(position, models): - delta_turn = convert_degrees_to_steps(ROTATED_POSITION_DEGREE, models) - nearest_pos = np.round(position.astype(float) / delta_turn) * delta_turn - return nearest_pos.astype(position.dtype) - - -def run_arm_calibration(arm: MotorsBus, robot_type: str, arm_name: str, arm_type: str): - """This function ensures that a neural network trained on data collected on a given robot - can work on another robot. For instance before calibration, setting a same goal position - for each motor of two different robots will get two very different positions. But after calibration, - the two robots will move to the same position.To this end, this function computes the homing offset - and the drive mode for each motor of a given robot. - - Homing offset is used to shift the motor position to a ]-2048, +2048[ nominal range (when the motor uses 2048 steps - to complete a half a turn). This range is set around an arbitrary "zero position" corresponding to all motor positions - being 0. During the calibration process, you will need to manually move the robot to this "zero position". - - Drive mode is used to invert the rotation direction of the motor. This is useful when some motors have been assembled - in the opposite orientation for some robots. During the calibration process, you will need to manually move the robot - to the "rotated position". - - After calibration, the homing offsets and drive modes are stored in a cache. - - Example of usage: - ```python - run_arm_calibration(arm, "koch", "left", "follower") - ``` - """ - if (arm.read("Torque_Enable") != TorqueMode.DISABLED.value).any(): - raise ValueError("To run calibration, the torque must be disabled on all motors.") - - print(f"\nRunning calibration of {robot_type} {arm_name} {arm_type}...") - - 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.models) - - # Compute homing offset so that `present_position + homing_offset ~= target_position`. - zero_pos = arm.read("Present_Position") - zero_nearest_pos = compute_nearest_rounded_position(zero_pos, arm.models) - homing_offset = zero_target_pos - zero_nearest_pos - - # 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 arbitrarily 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.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) - - # Re-compute homing offset to take into account drive mode - rotated_drived_pos = apply_drive_mode(rotated_pos, drive_mode) - rotated_nearest_pos = compute_nearest_rounded_position(rotated_drived_pos, arm.models) - homing_offset = rotated_target_pos - rotated_nearest_pos - - print("\nMove arm to rest position") - print("See: " + URL_TEMPLATE.format(robot=robot_type, arm=arm_type, position="rest")) - input("Press Enter to continue...") - print() - - # Joints with rotational motions are expressed in degrees in nominal range of [-180, 180] - calib_mode = [MotorNormMode.DEGREE.name] * len(arm.names) - - # TODO(rcadene): make type of joints (DEGREE or LINEAR) configurable from yaml? - if robot_type in ["aloha"] and "gripper" in arm.names: - # Joints with linear motions (like gripper of Aloha) are expressed in nominal range of [0, 100] - calib_idx = arm.names.index("gripper") - calib_mode[calib_idx] = MotorNormMode.LINEAR.name - - calib_data = { - "homing_offset": homing_offset.tolist(), - "drive_mode": drive_mode.tolist(), - "start_pos": zero_pos.tolist(), - "end_pos": rotated_pos.tolist(), - "calib_mode": calib_mode, - "motor_names": arm.names, - } - return calib_data