forked from tangger/lerobot
195 lines
6.1 KiB
Python
195 lines
6.1 KiB
Python
# 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.
|
|
|
|
from copy import deepcopy
|
|
|
|
from ..motors_bus import MotorsBus
|
|
|
|
PROTOCOL_VERSION = 0
|
|
BAUDRATE = 1_000_000
|
|
DEFAULT_TIMEOUT_MS = 1000
|
|
|
|
MAX_ID_RANGE = 252
|
|
|
|
# For joints in percentage (i.e. joints that move linearly like the prismatic joint of a gripper),
|
|
# their nominal range is [0, 100] %. For instance, for Aloha gripper, 0% is fully
|
|
# closed, and 100% is fully open. To account for slight calibration issue, we allow up to
|
|
# [-10, 110] until an error is raised.
|
|
LOWER_BOUND_LINEAR = -10
|
|
UPPER_BOUND_LINEAR = 110
|
|
|
|
HALF_TURN_DEGREE = 180
|
|
|
|
# See this link for STS3215 Memory Table:
|
|
# https://docs.google.com/spreadsheets/d/1GVs7W1VS1PqdhA1nW-abeyAHhTUxKUdR/edit?usp=sharing&ouid=116566590112741600240&rtpof=true&sd=true
|
|
# data_name: (address, size_byte)
|
|
SCS_SERIES_CONTROL_TABLE = {
|
|
"Model": (3, 2),
|
|
"ID": (5, 1),
|
|
"Baud_Rate": (6, 1),
|
|
"Return_Delay": (7, 1),
|
|
"Response_Status_Level": (8, 1),
|
|
"Min_Angle_Limit": (9, 2),
|
|
"Max_Angle_Limit": (11, 2),
|
|
"Max_Temperature_Limit": (13, 1),
|
|
"Max_Voltage_Limit": (14, 1),
|
|
"Min_Voltage_Limit": (15, 1),
|
|
"Max_Torque_Limit": (16, 2),
|
|
"Phase": (18, 1),
|
|
"Unloading_Condition": (19, 1),
|
|
"LED_Alarm_Condition": (20, 1),
|
|
"P_Coefficient": (21, 1),
|
|
"D_Coefficient": (22, 1),
|
|
"I_Coefficient": (23, 1),
|
|
"Minimum_Startup_Force": (24, 2),
|
|
"CW_Dead_Zone": (26, 1),
|
|
"CCW_Dead_Zone": (27, 1),
|
|
"Protection_Current": (28, 2),
|
|
"Angular_Resolution": (30, 1),
|
|
"Offset": (31, 2),
|
|
"Mode": (33, 1),
|
|
"Protective_Torque": (34, 1),
|
|
"Protection_Time": (35, 1),
|
|
"Overload_Torque": (36, 1),
|
|
"Speed_closed_loop_P_proportional_coefficient": (37, 1),
|
|
"Over_Current_Protection_Time": (38, 1),
|
|
"Velocity_closed_loop_I_integral_coefficient": (39, 1),
|
|
"Torque_Enable": (40, 1),
|
|
"Acceleration": (41, 1),
|
|
"Goal_Position": (42, 2),
|
|
"Goal_Time": (44, 2),
|
|
"Goal_Speed": (46, 2),
|
|
"Torque_Limit": (48, 2),
|
|
"Lock": (55, 1),
|
|
"Present_Position": (56, 2),
|
|
"Present_Speed": (58, 2),
|
|
"Present_Load": (60, 2),
|
|
"Present_Voltage": (62, 1),
|
|
"Present_Temperature": (63, 1),
|
|
"Status": (65, 1),
|
|
"Moving": (66, 1),
|
|
"Present_Current": (69, 2),
|
|
# Not in the Memory Table
|
|
"Maximum_Acceleration": (85, 2),
|
|
}
|
|
|
|
SCS_SERIES_BAUDRATE_TABLE = {
|
|
0: 1_000_000,
|
|
1: 500_000,
|
|
2: 250_000,
|
|
3: 128_000,
|
|
4: 115_200,
|
|
5: 57_600,
|
|
6: 38_400,
|
|
7: 19_200,
|
|
}
|
|
|
|
CALIBRATION_REQUIRED = ["Goal_Position", "Present_Position"]
|
|
|
|
MODEL_CONTROL_TABLE = {
|
|
"scs_series": SCS_SERIES_CONTROL_TABLE,
|
|
"sts3215": SCS_SERIES_CONTROL_TABLE,
|
|
}
|
|
|
|
MODEL_RESOLUTION = {
|
|
"scs_series": 4096,
|
|
"sts3215": 4096,
|
|
}
|
|
|
|
MODEL_BAUDRATE_TABLE = {
|
|
"scs_series": SCS_SERIES_BAUDRATE_TABLE,
|
|
"sts3215": SCS_SERIES_BAUDRATE_TABLE,
|
|
}
|
|
|
|
# High number of retries is needed for feetech compared to dynamixel motors.
|
|
NUM_READ_RETRY = 20
|
|
NUM_WRITE_RETRY = 20
|
|
|
|
|
|
class FeetechMotorsBus(MotorsBus):
|
|
"""
|
|
The FeetechMotorsBus class allows to efficiently read and write to the attached motors. It relies on the
|
|
python feetech sdk to communicate with the motors, which is itself based on the dynamixel sdk.
|
|
"""
|
|
|
|
model_ctrl_table = deepcopy(MODEL_CONTROL_TABLE)
|
|
model_resolution_table = deepcopy(MODEL_RESOLUTION)
|
|
model_baudrate_table = deepcopy(MODEL_BAUDRATE_TABLE)
|
|
calibration_required = deepcopy(CALIBRATION_REQUIRED)
|
|
default_timeout = DEFAULT_TIMEOUT_MS
|
|
|
|
def __init__(
|
|
self,
|
|
port: str,
|
|
motors: dict[str, tuple[int, str]],
|
|
):
|
|
super().__init__(port, motors)
|
|
import scservo_sdk as scs
|
|
|
|
self.port_handler = scs.PortHandler(self.port)
|
|
self.packet_handler = scs.PacketHandler(PROTOCOL_VERSION)
|
|
self.reader = scs.GroupSyncRead(self.port_handler, self.packet_handler, 0, 0)
|
|
self.writer = scs.GroupSyncWrite(self.port_handler, self.packet_handler, 0, 0)
|
|
|
|
def broadcast_ping(self, num_retry: int | None = None):
|
|
raise NotImplementedError # TODO
|
|
|
|
def calibrate_values(self, ids_values: dict[int, int]) -> dict[int, float]:
|
|
# TODO
|
|
return ids_values
|
|
|
|
def uncalibrate_values(self, ids_values: dict[int, float]) -> dict[int, int]:
|
|
# TODO
|
|
return ids_values
|
|
|
|
def _is_comm_success(self, comm: int) -> bool:
|
|
import scservo_sdk as scs
|
|
|
|
return comm == scs.COMM_SUCCESS
|
|
|
|
@staticmethod
|
|
def split_int_bytes(value: int, n_bytes: int) -> list[int]:
|
|
# Validate input
|
|
if value < 0:
|
|
raise ValueError(f"Negative values are not allowed: {value}")
|
|
|
|
max_value = {1: 0xFF, 2: 0xFFFF, 4: 0xFFFFFFFF}.get(n_bytes)
|
|
if max_value is None:
|
|
raise NotImplementedError(f"Unsupported byte size: {n_bytes}. Expected [1, 2, 4].")
|
|
|
|
if value > max_value:
|
|
raise ValueError(f"Value {value} exceeds the maximum for {n_bytes} bytes ({max_value}).")
|
|
|
|
import scservo_sdk as scs
|
|
|
|
# Note: No need to convert back into unsigned int, since this byte preprocessing
|
|
# already handles it for us.
|
|
if n_bytes == 1:
|
|
data = [
|
|
scs.SCS_LOBYTE(scs.SCS_LOWORD(value)),
|
|
]
|
|
elif n_bytes == 2:
|
|
data = [
|
|
scs.SCS_LOBYTE(scs.SCS_LOWORD(value)),
|
|
scs.SCS_HIBYTE(scs.SCS_LOWORD(value)),
|
|
]
|
|
elif n_bytes == 4:
|
|
data = [
|
|
scs.SCS_LOBYTE(scs.SCS_LOWORD(value)),
|
|
scs.SCS_HIBYTE(scs.SCS_LOWORD(value)),
|
|
scs.SCS_LOBYTE(scs.SCS_HIWORD(value)),
|
|
scs.SCS_HIBYTE(scs.SCS_HIWORD(value)),
|
|
]
|
|
return data
|