Add FeetechMotorsBus, SO-100, Moss-v1 #419
Merged
Conversation
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Cadene
force-pushed
the
user/rcadene/2024_09_04_feetech
branch
from
f6b661d to
9d23d04
Compare
Cadene
commented
Sep 17, 2024
added 4 commits
September 17, 2024 11:42
…ace/lerobot into user/rcadene/2024_09_04_feetech
jess-moss
force-pushed
the
user/rcadene/2024_09_04_feetech
branch
from
9c6ebce to
aa22946
Compare
added 3 commits
September 17, 2024 17:49
… in the camera driver classes.
… in the camera driver classes. Fixed test_cameras.py script so it passes.
jess-moss
reviewed
Sep 20, 2024
Base automatically changed from
user/rcadene/2024_10_12_refactor_record
to
main
October 16, 2024 18:51
Cadene
commented
Oct 23, 2024
Comment on lines
-232
to
-260
| def find_available_ports(): | ||
| ports = [] | ||
| for path in Path("/dev").glob("tty*"): | ||
| ports.append(str(path)) | ||
| return ports | ||
|
|
||
|
|
||
| def find_port(): | ||
| print("Finding all available ports for the DynamixelMotorsBus.") | ||
| ports_before = find_available_ports() | ||
| print(ports_before) | ||
|
|
||
| print("Remove the usb cable from your DynamixelMotorsBus and press Enter when done.") | ||
| input() | ||
|
|
||
| time.sleep(0.5) | ||
| ports_after = find_available_ports() | ||
| ports_diff = list(set(ports_before) - set(ports_after)) | ||
|
|
||
| if len(ports_diff) == 1: | ||
| port = ports_diff[0] | ||
| print(f"The port of this DynamixelMotorsBus is '{port}'") | ||
| print("Reconnect the usb cable.") | ||
| elif len(ports_diff) == 0: | ||
| raise OSError(f"Could not detect the port. No difference was found ({ports_diff}).") | ||
| else: | ||
| raise OSError(f"Could not detect the port. More than one port was found ({ports_diff}).") | ||
|
|
||
|
|
There was a problem hiding this comment.
Moved to scripts/find_motors_bus_port.py
Comment on lines
-381
to
-394
| # Set expected baudrate for the bus | ||
| self.set_bus_baudrate(BAUDRATE) | ||
|
|
||
| if not self.are_motors_configured(): | ||
| input( | ||
| "\n/!\\ A configuration issue has been detected with your motors: \n" | ||
| "If it's the first time that you use these motors, press enter to configure your motors... but before " | ||
| "verify that all the cables are connected the proper way. If you find an issue, before making a modification, " | ||
| "kill the python process, unplug the power cord to not damage the motors, rewire correctly, then plug the power " | ||
| "again and relaunch the script.\n" | ||
| ) | ||
| print() | ||
| self.configure_motors() | ||
|
|
There was a problem hiding this comment.
Moved to scripts/configure_motor.py
Comment on lines
531
to
381
| present_idx = self._read_with_motor_ids(self.motor_models, [idx], "ID")[0] | ||
| present_idx = self.read_with_motor_ids(self.motor_models, [idx], "ID")[0] |
There was a problem hiding this comment.
Made it public since it is used outside of the class by scripts/configure_motor.py (same for feetech)
Comment on lines
-22
to
-144
| # 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 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.motor_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.motor_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 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) | ||
|
|
||
| # 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.motor_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 = [CalibrationMode.DEGREE.name] * len(arm.motor_names) | ||
|
|
||
| # TODO(rcadene): make type of joints (DEGREE or LINEAR) configurable from yaml? | ||
| if robot_type == "aloha" and "gripper" in arm.motor_names: | ||
| # Joints with linear motions (like gripper of Aloha) are experessed in nominal range of [0, 100] | ||
| calib_idx = arm.motor_names.index("gripper") | ||
| calib_mode[calib_idx] = CalibrationMode.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.motor_names, | ||
| } | ||
| return calib_data | ||
|
|
There was a problem hiding this comment.
Moved to lerobot/common/robot_devices/robots/dynamixel_calibration.py
ramkumarkoppu
approved these changes
Oct 24, 2024
astroyat
pushed a commit
to astroyat/lerobot
that referenced
this pull request
Nov 23, 2024
Co-authored-by: jess-moss <jess.moss@huggingface.co> Co-authored-by: Simon Alibert <75076266+aliberts@users.noreply.github.com>
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
What this does
Feetech motor bus, SO-100, Moss-v1
lerobot/common/robot_devices/motors/feetech.pylerobot/configs/robot/so100.yamlandlerobot/configs/robot/moss.yamlexamples/10_use_so100.mdandexamples/11_use_moss.mdlerobot/scripts/find_motors_bus_port.pyscripts/configure_motor.pyAuto-calibration
lerobot/common/robot_devices/robots/dynamixel_calibration.pylerobot/common/robot_devices/robots/feetech_calibration.pywith autocalibration for SO-100 and Moss-v1 follower armsMinor
read_with_motor_ids(...num_retry=NUM_READ_RETRYandwrite_with_motor_ids(...num_retry=NUM_WRITE_RETRY) to set them to a low value of retries duringconfigure_motor.pyand thus speedup configuration processSee tutorial: https://github.com/huggingface/lerobot/blob/1d92acf1e3d6123874605d8048e110d58a383868/examples/10_use_so100.md
How it was tested
https://huggingface.co/spaces/lerobot/visualize_dataset?dataset=cadene%2Fmoss_debug&episode=0
https://huggingface.co/spaces/lerobot/visualize_dataset?dataset=cadene%2Fso100_debug&episode=0
How to checkout & try? (for the reviewer)