Remove deprecated dynamixel_calibration
This commit is contained in:
Simon Alibert
@@ -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
|
|
||||||
Reference in New Issue
Block a user