b31283ed6c
test by running using calibration and then run visualize calibration and each non linear joint has value of 2047 in middle at homed position, thus the 0<->4096 point is shifted for each motor to be on the other side of the homing position.
732 lines
26 KiB
Python
732 lines
26 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.
|
|
|
|
import enum
|
|
import time
|
|
import traceback
|
|
from copy import deepcopy
|
|
|
|
import numpy as np
|
|
import tqdm
|
|
|
|
from lerobot.common.robot_devices.motors.configs import FeetechMotorsBusConfig
|
|
from lerobot.common.robot_devices.utils import RobotDeviceAlreadyConnectedError, RobotDeviceNotConnectedError
|
|
from lerobot.common.utils.utils import capture_timestamp_utc
|
|
|
|
PROTOCOL_VERSION = 0
|
|
BAUDRATE = 1_000_000
|
|
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
|
|
|
|
|
|
def convert_ticks_to_degrees(ticks, model):
|
|
resolutions = MODEL_RESOLUTION[model]
|
|
# Convert the ticks to degrees
|
|
return ticks * (360.0 / resolutions)
|
|
|
|
|
|
def convert_degrees_to_ticks(degrees, model):
|
|
resolutions = MODEL_RESOLUTION[model]
|
|
# Convert degrees to motor ticks
|
|
return int(degrees * (resolutions / 360.0))
|
|
|
|
|
|
def adjusted_to_homing_ticks(raw_motor_ticks: int, model: str, motorbus, motor_id: int) -> int:
|
|
"""
|
|
Shifts raw [0..4095] ticks by an encoder offset, modulo a single turn [0..4095].
|
|
"""
|
|
resolutions = MODEL_RESOLUTION[model]
|
|
|
|
# Add offset and wrap within resolution
|
|
ticks = (raw_motor_ticks) % resolutions
|
|
|
|
# # Re-center into a symmetric range (e.g., [-2048, 2047] if resolutions==4096) Thus the middle homing position will be virtual 0.
|
|
if ticks > resolutions // 2:
|
|
ticks -= resolutions
|
|
|
|
# Update direction of rotation of the motor to match between leader and follower.
|
|
# In fact, the motor of the leader for a given joint can be assembled in an
|
|
# opposite direction in term of rotation than the motor of the follower on the same joint.
|
|
drive_mode = 0
|
|
if motorbus.calibration is not None:
|
|
drive_mode = motorbus.calibration["drive_mode"][motor_id - 1]
|
|
|
|
if drive_mode:
|
|
ticks *= -1
|
|
|
|
return ticks
|
|
|
|
|
|
def adjusted_to_motor_ticks(adjusted_pos: int, model: str, motorbus, motor_id: int) -> int:
|
|
"""
|
|
Inverse of adjusted_to_homing_ticks().
|
|
"""
|
|
# Update direction of rotation of the motor to match between leader and follower.
|
|
# In fact, the motor of the leader for a given joint can be assembled in an
|
|
# opposite direction in term of rotation than the motor of the follower on the same joint.
|
|
drive_mode = 0
|
|
if motorbus.calibration is not None:
|
|
drive_mode = motorbus.calibration["drive_mode"][motor_id - 1]
|
|
|
|
if drive_mode:
|
|
adjusted_pos *= -1
|
|
|
|
resolutions = MODEL_RESOLUTION[model]
|
|
|
|
# Remove offset and wrap within resolution
|
|
ticks = (adjusted_pos) % resolutions
|
|
|
|
return ticks
|
|
|
|
|
|
def convert_to_bytes(value, bytes, mock=False):
|
|
if mock:
|
|
return 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 bytes == 1:
|
|
data = [
|
|
scs.SCS_LOBYTE(scs.SCS_LOWORD(value)),
|
|
]
|
|
elif bytes == 2:
|
|
data = [
|
|
scs.SCS_LOBYTE(scs.SCS_LOWORD(value)),
|
|
scs.SCS_HIBYTE(scs.SCS_LOWORD(value)),
|
|
]
|
|
elif 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)),
|
|
]
|
|
else:
|
|
raise NotImplementedError(
|
|
f"Value of the number of bytes to be sent is expected to be in [1, 2, 4], but "
|
|
f"{bytes} is provided instead."
|
|
)
|
|
return data
|
|
|
|
|
|
def get_group_sync_key(data_name, motor_names):
|
|
group_key = f"{data_name}_" + "_".join(motor_names)
|
|
return group_key
|
|
|
|
|
|
def get_result_name(fn_name, data_name, motor_names):
|
|
group_key = get_group_sync_key(data_name, motor_names)
|
|
rslt_name = f"{fn_name}_{group_key}"
|
|
return rslt_name
|
|
|
|
|
|
def get_queue_name(fn_name, data_name, motor_names):
|
|
group_key = get_group_sync_key(data_name, motor_names)
|
|
queue_name = f"{fn_name}_{group_key}"
|
|
return queue_name
|
|
|
|
|
|
def get_log_name(var_name, fn_name, data_name, motor_names):
|
|
group_key = get_group_sync_key(data_name, motor_names)
|
|
log_name = f"{var_name}_{fn_name}_{group_key}"
|
|
return log_name
|
|
|
|
|
|
def assert_same_address(model_ctrl_table, motor_models, data_name):
|
|
all_addr = []
|
|
all_bytes = []
|
|
for model in motor_models:
|
|
addr, bytes = model_ctrl_table[model][data_name]
|
|
all_addr.append(addr)
|
|
all_bytes.append(bytes)
|
|
|
|
if len(set(all_addr)) != 1:
|
|
raise NotImplementedError(
|
|
f"At least two motor models use a different address for `data_name`='{data_name}' ({list(zip(motor_models, all_addr, strict=False))}). Contact a LeRobot maintainer."
|
|
)
|
|
|
|
if len(set(all_bytes)) != 1:
|
|
raise NotImplementedError(
|
|
f"At least two motor models use a different bytes representation for `data_name`='{data_name}' ({list(zip(motor_models, all_bytes, strict=False))}). Contact a LeRobot maintainer."
|
|
)
|
|
|
|
|
|
class TorqueMode(enum.Enum):
|
|
ENABLED = 1
|
|
DISABLED = 0
|
|
|
|
|
|
class DriveMode(enum.Enum):
|
|
NON_INVERTED = 0
|
|
INVERTED = 1
|
|
|
|
|
|
class CalibrationMode(enum.Enum):
|
|
# Joints with rotational motions are expressed in degrees in nominal range of [-180, 180]
|
|
DEGREE = 0
|
|
# Joints with linear motions (like gripper of Aloha) are expressed in nominal range of [0, 100]
|
|
LINEAR = 1
|
|
|
|
|
|
class JointOutOfRangeError(Exception):
|
|
def __init__(self, message="Joint is out of range"):
|
|
self.message = message
|
|
super().__init__(self.message)
|
|
|
|
|
|
class FeetechMotorsBus:
|
|
"""
|
|
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. For more info, see the [feetech SDK Documentation](https://emanual.robotis.com/docs/en/software/feetech/feetech_sdk/sample_code/python_read_write_protocol_2_0/#python-read-write-protocol-20).
|
|
|
|
A FeetechMotorsBus instance requires a port (e.g. `FeetechMotorsBus(port="/dev/tty.usbmodem575E0031751"`)).
|
|
To find the port, you can run our utility script:
|
|
```bash
|
|
python lerobot/scripts/find_motors_bus_port.py
|
|
>>> Finding all available ports for the MotorsBus.
|
|
>>> ['/dev/tty.usbmodem575E0032081', '/dev/tty.usbmodem575E0031751']
|
|
>>> Remove the usb cable from your FeetechMotorsBus and press Enter when done.
|
|
>>> The port of this FeetechMotorsBus is /dev/tty.usbmodem575E0031751.
|
|
>>> Reconnect the usb cable.
|
|
```
|
|
|
|
Example of usage for 1 motor connected to the bus:
|
|
```python
|
|
motor_name = "gripper"
|
|
motor_index = 6
|
|
motor_model = "sts3215"
|
|
|
|
config = FeetechMotorsBusConfig(
|
|
port="/dev/tty.usbmodem575E0031751",
|
|
motors={motor_name: (motor_index, motor_model)},
|
|
)
|
|
motors_bus = FeetechMotorsBus(config)
|
|
motors_bus.connect()
|
|
|
|
position = motors_bus.read("Present_Position")
|
|
|
|
# move from a few motor steps as an example
|
|
few_steps = 30
|
|
motors_bus.write("Goal_Position", position + few_steps)
|
|
|
|
# when done, consider disconnecting
|
|
motors_bus.disconnect()
|
|
```
|
|
"""
|
|
|
|
def __init__(
|
|
self,
|
|
config: FeetechMotorsBusConfig,
|
|
):
|
|
self.port = config.port
|
|
self.motors = config.motors
|
|
self.mock = config.mock
|
|
|
|
self.model_ctrl_table = deepcopy(MODEL_CONTROL_TABLE)
|
|
self.model_resolution = deepcopy(MODEL_RESOLUTION)
|
|
|
|
self.port_handler = None
|
|
self.packet_handler = None
|
|
self.calibration = None
|
|
self.is_connected = False
|
|
self.group_readers = {}
|
|
self.group_writers = {}
|
|
self.logs = {}
|
|
|
|
def connect(self):
|
|
if self.is_connected:
|
|
raise RobotDeviceAlreadyConnectedError(
|
|
f"FeetechMotorsBus({self.port}) is already connected. Do not call `motors_bus.connect()` twice."
|
|
)
|
|
|
|
if self.mock:
|
|
import tests.mock_scservo_sdk as scs
|
|
else:
|
|
import scservo_sdk as scs
|
|
|
|
self.port_handler = scs.PortHandler(self.port)
|
|
self.packet_handler = scs.PacketHandler(PROTOCOL_VERSION)
|
|
|
|
try:
|
|
if not self.port_handler.openPort():
|
|
raise OSError(f"Failed to open port '{self.port}'.")
|
|
except Exception:
|
|
traceback.print_exc()
|
|
print(
|
|
"\nTry running `python lerobot/scripts/find_motors_bus_port.py` to make sure you are using the correct port.\n"
|
|
)
|
|
raise
|
|
|
|
# Allow to read and write
|
|
self.is_connected = True
|
|
|
|
self.port_handler.setPacketTimeoutMillis(TIMEOUT_MS)
|
|
|
|
def reconnect(self):
|
|
if self.mock:
|
|
import tests.mock_scservo_sdk as scs
|
|
else:
|
|
import scservo_sdk as scs
|
|
|
|
self.port_handler = scs.PortHandler(self.port)
|
|
self.packet_handler = scs.PacketHandler(PROTOCOL_VERSION)
|
|
|
|
if not self.port_handler.openPort():
|
|
raise OSError(f"Failed to open port '{self.port}'.")
|
|
|
|
self.is_connected = True
|
|
|
|
def are_motors_configured(self):
|
|
# Only check the motor indices and not baudrate, since if the motor baudrates are incorrect,
|
|
# a ConnectionError will be raised anyway.
|
|
try:
|
|
return (self.motor_indices == self.read("ID")).all()
|
|
except ConnectionError as e:
|
|
print(e)
|
|
return False
|
|
|
|
def find_motor_indices(self, possible_ids=None, num_retry=2):
|
|
if possible_ids is None:
|
|
possible_ids = range(MAX_ID_RANGE)
|
|
|
|
indices = []
|
|
for idx in tqdm.tqdm(possible_ids):
|
|
try:
|
|
present_idx = self.read_with_motor_ids(self.motor_models, [idx], "ID", num_retry=num_retry)[0]
|
|
except ConnectionError:
|
|
continue
|
|
|
|
if idx != present_idx:
|
|
# sanity check
|
|
raise OSError(
|
|
"Motor index used to communicate through the bus is not the same as the one present in the motor memory. The motor memory might be damaged."
|
|
)
|
|
indices.append(idx)
|
|
|
|
return indices
|
|
|
|
def set_bus_baudrate(self, baudrate):
|
|
present_bus_baudrate = self.port_handler.getBaudRate()
|
|
if present_bus_baudrate != baudrate:
|
|
print(f"Setting bus baud rate to {baudrate}. Previously {present_bus_baudrate}.")
|
|
self.port_handler.setBaudRate(baudrate)
|
|
|
|
if self.port_handler.getBaudRate() != baudrate:
|
|
raise OSError("Failed to write bus baud rate.")
|
|
|
|
@property
|
|
def motor_names(self) -> list[str]:
|
|
return list(self.motors.keys())
|
|
|
|
@property
|
|
def motor_models(self) -> list[str]:
|
|
return [model for _, model in self.motors.values()]
|
|
|
|
@property
|
|
def motor_indices(self) -> list[int]:
|
|
return [idx for idx, _ in self.motors.values()]
|
|
|
|
def set_calibration(self, calibration: dict[str, list]):
|
|
self.calibration = calibration
|
|
|
|
def apply_calibration(self, values: np.ndarray | list, motor_names: list[str] | None):
|
|
if motor_names is None:
|
|
motor_names = self.motor_names
|
|
|
|
# Convert from unsigned int32 original range [0, 2**32] to signed float32 range
|
|
values = values.astype(np.float32)
|
|
|
|
for i, name in enumerate(motor_names):
|
|
calib_idx = self.calibration["motor_names"].index(name)
|
|
calib_mode = self.calibration["calib_mode"][calib_idx]
|
|
|
|
if CalibrationMode[calib_mode] == CalibrationMode.DEGREE:
|
|
motor_idx, model = self.motors[name]
|
|
|
|
# Convert raw motor ticks to homed ticks, then convert the homed ticks to degrees
|
|
values[i] = adjusted_to_homing_ticks(values[i], model, self, motor_idx)
|
|
values[i] = convert_ticks_to_degrees(values[i], model)
|
|
|
|
elif CalibrationMode[calib_mode] == CalibrationMode.LINEAR:
|
|
start_pos = self.calibration["start_pos"][calib_idx]
|
|
end_pos = self.calibration["end_pos"][calib_idx]
|
|
|
|
# Rescale the present position to a nominal range [0, 100] %,
|
|
# useful for joints with linear motions like Aloha gripper
|
|
values[i] = (values[i] - start_pos) / (end_pos - start_pos) * 100
|
|
|
|
if (values[i] < LOWER_BOUND_LINEAR) or (values[i] > UPPER_BOUND_LINEAR):
|
|
raise JointOutOfRangeError(
|
|
f"Wrong motor position range detected for {name}. "
|
|
f"Expected to be in nominal range of [0, 100] % (a full linear translation), "
|
|
f"with a maximum range of [{LOWER_BOUND_LINEAR}, {UPPER_BOUND_LINEAR}] % to account for some imprecision during calibration, "
|
|
f"but present value is {values[i]} %. "
|
|
"This might be due to a cable connection issue creating an artificial jump in motor values. "
|
|
"You need to recalibrate by running: `python lerobot/scripts/control_robot.py calibrate`"
|
|
)
|
|
|
|
return values
|
|
|
|
def revert_calibration(self, values: np.ndarray | list, motor_names: list[str] | None):
|
|
"""Inverse of `apply_calibration`."""
|
|
if motor_names is None:
|
|
motor_names = self.motor_names
|
|
|
|
for i, name in enumerate(motor_names):
|
|
calib_idx = self.calibration["motor_names"].index(name)
|
|
calib_mode = self.calibration["calib_mode"][calib_idx]
|
|
|
|
if CalibrationMode[calib_mode] == CalibrationMode.DEGREE:
|
|
motor_idx, model = self.motors[name]
|
|
|
|
# Convert degrees to homed ticks, then convert the homed ticks to raw ticks
|
|
values[i] = convert_degrees_to_ticks(values[i], model)
|
|
values[i] = adjusted_to_motor_ticks(values[i], model, self, motor_idx)
|
|
|
|
elif CalibrationMode[calib_mode] == CalibrationMode.LINEAR:
|
|
start_pos = self.calibration["start_pos"][calib_idx]
|
|
end_pos = self.calibration["end_pos"][calib_idx]
|
|
|
|
# Convert from nominal lnear range of [0, 100] % to
|
|
# actual motor range of values which can be arbitrary.
|
|
values[i] = values[i] / 100 * (end_pos - start_pos) + start_pos
|
|
|
|
values = np.round(values).astype(np.int32)
|
|
return values
|
|
|
|
def read_with_motor_ids(self, motor_models, motor_ids, data_name, num_retry=NUM_READ_RETRY):
|
|
if self.mock:
|
|
import tests.mock_scservo_sdk as scs
|
|
else:
|
|
import scservo_sdk as scs
|
|
|
|
return_list = True
|
|
if not isinstance(motor_ids, list):
|
|
return_list = False
|
|
motor_ids = [motor_ids]
|
|
|
|
assert_same_address(self.model_ctrl_table, self.motor_models, data_name)
|
|
addr, bytes = self.model_ctrl_table[motor_models[0]][data_name]
|
|
group = scs.GroupSyncRead(self.port_handler, self.packet_handler, addr, bytes)
|
|
for idx in motor_ids:
|
|
group.addParam(idx)
|
|
|
|
for _ in range(num_retry):
|
|
comm = group.txRxPacket()
|
|
if comm == scs.COMM_SUCCESS:
|
|
break
|
|
|
|
if comm != scs.COMM_SUCCESS:
|
|
raise ConnectionError(
|
|
f"Read failed due to communication error on port {self.port_handler.port_name} for indices {motor_ids}: "
|
|
f"{self.packet_handler.getTxRxResult(comm)}"
|
|
)
|
|
|
|
values = []
|
|
for idx in motor_ids:
|
|
value = group.getData(idx, addr, bytes)
|
|
values.append(value)
|
|
|
|
if return_list:
|
|
return values
|
|
else:
|
|
return values[0]
|
|
|
|
def read(self, data_name, motor_names: str | list[str] | None = None):
|
|
if self.mock:
|
|
import tests.mock_scservo_sdk as scs
|
|
else:
|
|
import scservo_sdk as scs
|
|
|
|
if not self.is_connected:
|
|
raise RobotDeviceNotConnectedError(
|
|
f"FeetechMotorsBus({self.port}) is not connected. You need to run `motors_bus.connect()`."
|
|
)
|
|
|
|
start_time = time.perf_counter()
|
|
|
|
if motor_names is None:
|
|
motor_names = self.motor_names
|
|
|
|
if isinstance(motor_names, str):
|
|
motor_names = [motor_names]
|
|
|
|
motor_ids = []
|
|
models = []
|
|
for name in motor_names:
|
|
motor_idx, model = self.motors[name]
|
|
motor_ids.append(motor_idx)
|
|
models.append(model)
|
|
|
|
assert_same_address(self.model_ctrl_table, models, data_name)
|
|
addr, bytes = self.model_ctrl_table[model][data_name]
|
|
group_key = get_group_sync_key(data_name, motor_names)
|
|
|
|
if data_name not in self.group_readers:
|
|
# Very Important to flush the buffer!
|
|
self.port_handler.ser.reset_output_buffer()
|
|
self.port_handler.ser.reset_input_buffer()
|
|
|
|
# Create new group reader
|
|
self.group_readers[group_key] = scs.GroupSyncRead(
|
|
self.port_handler, self.packet_handler, addr, bytes
|
|
)
|
|
for idx in motor_ids:
|
|
self.group_readers[group_key].addParam(idx)
|
|
|
|
for _ in range(NUM_READ_RETRY):
|
|
comm = self.group_readers[group_key].txRxPacket()
|
|
if comm == scs.COMM_SUCCESS:
|
|
break
|
|
|
|
if comm != scs.COMM_SUCCESS:
|
|
raise ConnectionError(
|
|
f"Read failed due to communication error on port {self.port} for group_key {group_key}: "
|
|
f"{self.packet_handler.getTxRxResult(comm)}"
|
|
)
|
|
|
|
values = []
|
|
for idx in motor_ids:
|
|
value = self.group_readers[group_key].getData(idx, addr, bytes)
|
|
values.append(value)
|
|
|
|
values = np.array(values)
|
|
|
|
if data_name in CALIBRATION_REQUIRED and self.calibration is not None:
|
|
values = self.apply_calibration(values, motor_names)
|
|
|
|
# log the number of seconds it took to read the data from the motors
|
|
delta_ts_name = get_log_name("delta_timestamp_s", "read", data_name, motor_names)
|
|
self.logs[delta_ts_name] = time.perf_counter() - start_time
|
|
|
|
# log the utc time at which the data was received
|
|
ts_utc_name = get_log_name("timestamp_utc", "read", data_name, motor_names)
|
|
self.logs[ts_utc_name] = capture_timestamp_utc()
|
|
|
|
return values
|
|
|
|
def write_with_motor_ids(self, motor_models, motor_ids, data_name, values, num_retry=NUM_WRITE_RETRY):
|
|
if self.mock:
|
|
import tests.mock_scservo_sdk as scs
|
|
else:
|
|
import scservo_sdk as scs
|
|
|
|
if not isinstance(motor_ids, list):
|
|
motor_ids = [motor_ids]
|
|
if not isinstance(values, list):
|
|
values = [values]
|
|
|
|
assert_same_address(self.model_ctrl_table, motor_models, data_name)
|
|
addr, bytes = self.model_ctrl_table[motor_models[0]][data_name]
|
|
group = scs.GroupSyncWrite(self.port_handler, self.packet_handler, addr, bytes)
|
|
for idx, value in zip(motor_ids, values, strict=True):
|
|
data = convert_to_bytes(value, bytes, self.mock)
|
|
group.addParam(idx, data)
|
|
|
|
for _ in range(num_retry):
|
|
comm = group.txPacket()
|
|
if comm == scs.COMM_SUCCESS:
|
|
break
|
|
|
|
if comm != scs.COMM_SUCCESS:
|
|
raise ConnectionError(
|
|
f"Write failed due to communication error on port {self.port_handler.port_name} for indices {motor_ids}: "
|
|
f"{self.packet_handler.getTxRxResult(comm)}"
|
|
)
|
|
|
|
def write(self, data_name, values: int | float | np.ndarray, motor_names: str | list[str] | None = None):
|
|
if not self.is_connected:
|
|
raise RobotDeviceNotConnectedError(
|
|
f"FeetechMotorsBus({self.port}) is not connected. You need to run `motors_bus.connect()`."
|
|
)
|
|
|
|
start_time = time.perf_counter()
|
|
|
|
if self.mock:
|
|
import tests.mock_scservo_sdk as scs
|
|
else:
|
|
import scservo_sdk as scs
|
|
|
|
if motor_names is None:
|
|
motor_names = self.motor_names
|
|
|
|
if isinstance(motor_names, str):
|
|
motor_names = [motor_names]
|
|
|
|
if isinstance(values, (int, float, np.integer)):
|
|
values = [int(values)] * len(motor_names)
|
|
|
|
values = np.array(values)
|
|
|
|
motor_ids = []
|
|
models = []
|
|
for name in motor_names:
|
|
motor_idx, model = self.motors[name]
|
|
motor_ids.append(motor_idx)
|
|
models.append(model)
|
|
|
|
if data_name in CALIBRATION_REQUIRED and self.calibration is not None:
|
|
values = self.revert_calibration(values, motor_names)
|
|
|
|
values = values.tolist()
|
|
|
|
assert_same_address(self.model_ctrl_table, models, data_name)
|
|
addr, bytes = self.model_ctrl_table[model][data_name]
|
|
group_key = get_group_sync_key(data_name, motor_names)
|
|
|
|
init_group = data_name not in self.group_readers
|
|
if init_group:
|
|
self.group_writers[group_key] = scs.GroupSyncWrite(
|
|
self.port_handler, self.packet_handler, addr, bytes
|
|
)
|
|
|
|
for idx, value in zip(motor_ids, values, strict=True):
|
|
data = convert_to_bytes(value, bytes, self.mock)
|
|
if init_group:
|
|
self.group_writers[group_key].addParam(idx, data)
|
|
else:
|
|
self.group_writers[group_key].changeParam(idx, data)
|
|
|
|
comm = self.group_writers[group_key].txPacket()
|
|
if comm != scs.COMM_SUCCESS:
|
|
raise ConnectionError(
|
|
f"Write failed due to communication error on port {self.port} for group_key {group_key}: "
|
|
f"{self.packet_handler.getTxRxResult(comm)}"
|
|
)
|
|
|
|
# log the number of seconds it took to write the data to the motors
|
|
delta_ts_name = get_log_name("delta_timestamp_s", "write", data_name, motor_names)
|
|
self.logs[delta_ts_name] = time.perf_counter() - start_time
|
|
|
|
# TODO(rcadene): should we log the time before sending the write command?
|
|
# log the utc time when the write has been completed
|
|
ts_utc_name = get_log_name("timestamp_utc", "write", data_name, motor_names)
|
|
self.logs[ts_utc_name] = capture_timestamp_utc()
|
|
|
|
def disconnect(self):
|
|
if not self.is_connected:
|
|
raise RobotDeviceNotConnectedError(
|
|
f"FeetechMotorsBus({self.port}) is not connected. Try running `motors_bus.connect()` first."
|
|
)
|
|
|
|
if self.port_handler is not None:
|
|
self.port_handler.closePort()
|
|
self.port_handler = None
|
|
|
|
self.packet_handler = None
|
|
self.group_readers = {}
|
|
self.group_writers = {}
|
|
self.is_connected = False
|
|
|
|
def __del__(self):
|
|
if getattr(self, "is_connected", False):
|
|
self.disconnect()
|