wangzixiang
/
lerobot
mirror of https://github.com/huggingface/lerobot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

WIP

This commit is contained in:
Remi Cadene 2024-11-27 13:59:37 +01:00
parent 9dd4414c6e
commit 2c45660d77
5 changed files with 686 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

320
examples/test.py Normal file
View File

@ -0,0 +1,320 @@
import time
from typing import Callable
import cv2
import numpy as np
from qai_hub_models.models.mediapipe_hand.app import MediaPipeHandApp
from qai_hub_models.models.mediapipe_hand.model import (
MediaPipeHand,
)
from qai_hub_models.utils.image_processing import (
app_to_net_image_inputs,
)
from lerobot.common.robot_devices.motors.feetech import FeetechMotorsBus
class HopeJuniorRobot:
def __init__(self):
self.arm_bus = FeetechMotorsBus(
port="/dev/tty.usbserial-2110",
motors={
# "motor1": (2, "sts3250"),
# "motor2": (1, "scs0009"),
"shoulder_pitch": [1, "sts3250"],
"shoulder_yaw": [2, "sts3215"], # TODO: sts3250
"shoulder_roll": [3, "sts3215"], # TODO: sts3250
"elbow_flex": [4, "sts3250"],
"wrist_roll": [5, "sts3215"],
"wrist_yaw": [6, "sts3215"],
"wrist_pitch": [7, "sts3215"],
},
protocol_version=0,
)
self.hand_bus = FeetechMotorsBus(
port="/dev/tty.usbserial-2140",
motors={
"thumb_basel_rotation": [30, "scs0009"],
"thumb_flexion": [27, "scs0009"],
"thumb_pinky_side": [26, "scs0009"],
"thumb_thumb_side": [28, "scs0009"],
"index_flexor": [25, "scs0009"],
"index_pinky_side": [31, "scs0009"],
"index_thumb_side": [32, "scs0009"],
"middle_flexor": [24, "scs0009"],
"middle_pinky_side": [33, "scs0009"],
"middle_thumb_side": [34, "scs0009"],
"ring_flexor": [21, "scs0009"],
"ring_pinky_side": [36, "scs0009"],
"ring_thumb_side": [35, "scs0009"],
"pinky_flexor": [23, "scs0009"],
"pinky_pinky_side": [38, "scs0009"],
"pinky_thumb_side": [37, "scs0009"],
},
protocol_version=1,
group_sync_read=False,
)
def connect(self):
self.arm_bus.connect()
self.hand_bus.connect()
ESCAPE_KEY_ID = 27
def capture_and_display_processed_frames(
frame_processor: Callable[[np.ndarray], np.ndarray],
window_display_name: str,
cap_device: int = 0,
) -> None:
"""
Capture frames from the given input camera device, run them through
the frame processor, and display the outputs in a window with the given name.
User should press Esc to exit.
Inputs:
frame_processor: Callable[[np.ndarray], np.ndarray]
Processes frames.
Input and output are numpy arrays of shape (H W C) with BGR channel layout and dtype uint8 / byte.
window_display_name: str
Name of the window used to display frames.
cap_device: int
Identifier for the camera to use to capture frames.
"""
cv2.namedWindow(window_display_name)
capture = cv2.VideoCapture(cap_device)
if not capture.isOpened():
raise ValueError("Unable to open video capture.")
frame_count = 0
has_frame, frame = capture.read()
while has_frame:
assert isinstance(frame, np.ndarray)
frame_count = frame_count + 1
# mirror frame
frame = np.ascontiguousarray(frame[:, ::-1, ::-1])
# process & show frame
processed_frame = frame_processor(frame)
cv2.imshow(window_display_name, processed_frame[:, :, ::-1])
has_frame, frame = capture.read()
key = cv2.waitKey(1)
if key == ESCAPE_KEY_ID:
break
capture.release()
def main():
robot = HopeJuniorRobot()
robot.connect()
print(robot.arm_bus.read("Present_Position"))
# print(motors_bus.write("Goal_Position", 500))
print(robot.hand_bus.read("Present_Position"))
# pos = hand_bus.read("Present_Position")
# hand_bus.write("Goal_Position", pos[0]+20, hand_bus.motor_names[0])
# hand_bus.write("Goal_Position", pos[i]+delta, hand_bus.motor_names[i])
robot.arm_bus.write("Torque_Enable", 1)
robot.arm_bus.write("Acceleration", 20)
robot.arm_bus.read("Acceleration")
robot.hand_bus.read("Acceleration")
robot.hand_bus.write("Acceleration", 10)
sleep = 1
robot.arm_bus.write("Goal_Position", [1981, 2030, 2069, 2032, 1874, 1957, 1695])
robot.hand_bus.write(
"Goal_Position",
[500, 1000, 0, 1000],
["thumb_basel_rotation", "thumb_flexion", "thumb_pinky_side", "thumb_thumb_side"],
)
time.sleep(sleep)
robot.hand_bus.write(
"Goal_Position", [100, 950, 100], ["index_flexor", "index_pinky_side", "index_thumb_side"]
)
time.sleep(sleep)
robot.hand_bus.write(
"Goal_Position", [100, 1000, 150], ["middle_flexor", "middle_pinky_side", "middle_thumb_side"]
)
time.sleep(sleep)
robot.hand_bus.write(
"Goal_Position", [200, 200, 0], ["ring_flexor", "ring_pinky_side", "ring_thumb_side"]
)
time.sleep(sleep)
robot.hand_bus.write(
"Goal_Position", [200, 100, 700], ["pinky_flexor", "pinky_pinky_side", "pinky_thumb_side"]
)
time.sleep(sleep)
time.sleep(3)
def move_arm(loop=10):
sleep = 1
for i in range(loop):
robot.arm_bus.write("Goal_Position", [1981, 2030, 2069, 2032, 1874, 1957, 1695])
time.sleep(sleep)
robot.arm_bus.write("Goal_Position", [1981, 2030, 2069, 2032, 1874, 1957, 1195])
time.sleep(sleep)
robot.arm_bus.write("Goal_Position", [1981, 2030, 2069, 2032, 1874, 1957, 2195])
time.sleep(sleep)
robot.arm_bus.write("Goal_Position", [1981, 2030, 2069, 2032, 1874, 1957, 1695])
time.sleep(sleep)
robot.arm_bus.write("Goal_Position", [1981, 2030, 2069, 2032, 1874, 1457, 1695])
time.sleep(sleep)
robot.arm_bus.write("Goal_Position", [1981, 2030, 2069, 2032, 1874, 2357, 1695])
time.sleep(sleep)
robot.arm_bus.write("Goal_Position", [1981, 2030, 2069, 2032, 1874, 1957, 1695])
time.sleep(sleep)
robot.arm_bus.write("Goal_Position", [1981, 2030, 2069, 2032, 974, 1957, 1695])
time.sleep(sleep)
robot.arm_bus.write("Goal_Position", [1981, 2030, 2069, 2032, 2674, 1957, 1695])
time.sleep(sleep + 2)
robot.arm_bus.write("Goal_Position", [1981, 2030, 2069, 2032, 1874, 1957, 1695])
time.sleep(sleep)
robot.arm_bus.write("Goal_Position", [1981, 2030, 2069, 1632, 1874, 1957, 1695])
time.sleep(sleep)
robot.arm_bus.write("Goal_Position", [1981, 2030, 1369, 1632, 1874, 1957, 1695])
time.sleep(sleep)
robot.arm_bus.write("Goal_Position", [1981, 2030, 2069, 2032, 1874, 1957, 1695])
time.sleep(sleep)
robot.arm_bus.write("Goal_Position", [1981, 1330, 2069, 2032, 1874, 1957, 1695])
time.sleep(sleep)
robot.arm_bus.write("Goal_Position", [1981, 2030, 2069, 2032, 1874, 1957, 1695])
time.sleep(sleep)
robot.arm_bus.write("Goal_Position", [2381, 2030, 2069, 2032, 1874, 1957, 1695])
time.sleep(sleep)
robot.arm_bus.write("Goal_Position", [1681, 2030, 2069, 2032, 1874, 1957, 1695])
time.sleep(sleep)
robot.arm_bus.write("Goal_Position", [1981, 2030, 2069, 2032, 1874, 1957, 1695])
time.sleep(sleep)
def move_hand(loop=10):
sleep = 0.5
for i in range(loop):
robot.hand_bus.write(
"Goal_Position",
[500, 1000, 0, 1000],
["thumb_basel_rotation", "thumb_flexion", "thumb_pinky_side", "thumb_thumb_side"],
)
time.sleep(sleep)
robot.hand_bus.write(
"Goal_Position", [100, 950, 100], ["index_flexor", "index_pinky_side", "index_thumb_side"]
)
time.sleep(sleep)
robot.hand_bus.write(
"Goal_Position", [100, 1000, 150], ["middle_flexor", "middle_pinky_side", "middle_thumb_side"]
)
time.sleep(sleep)
robot.hand_bus.write(
"Goal_Position", [200, 200, 0], ["ring_flexor", "ring_pinky_side", "ring_thumb_side"]
)
time.sleep(sleep)
robot.hand_bus.write(
"Goal_Position", [200, 100, 700], ["pinky_flexor", "pinky_pinky_side", "pinky_thumb_side"]
)
time.sleep(sleep)
robot.hand_bus.write(
"Goal_Position",
[500, 1000 - 250, 0 + 300, 1000 - 200],
["thumb_basel_rotation", "thumb_flexion", "thumb_pinky_side", "thumb_thumb_side"],
)
time.sleep(sleep)
robot.hand_bus.write(
"Goal_Position",
[100 + 450, 950 - 400, 100 + 400],
["index_flexor", "index_pinky_side", "index_thumb_side"],
)
time.sleep(sleep)
robot.hand_bus.write(
"Goal_Position",
[100 + 350, 1000 - 450, 150 + 450],
["middle_flexor", "middle_pinky_side", "middle_thumb_side"],
)
time.sleep(sleep)
robot.hand_bus.write(
"Goal_Position",
[200 + 650, 200 + 350, 0 + 350],
["ring_flexor", "ring_pinky_side", "ring_thumb_side"],
)
time.sleep(sleep)
robot.hand_bus.write(
"Goal_Position",
[200 + 450, 100 + 400, 700 - 400],
["pinky_flexor", "pinky_pinky_side", "pinky_thumb_side"],
)
time.sleep(sleep)
move_hand(3)
move_arm(1)
from concurrent.futures import ThreadPoolExecutor
with ThreadPoolExecutor() as executor:
executor.submit(move_arm)
executor.submit(move_hand)
breakpoint()
# # initial position
# for i in range(3):
# robot.hand_bus.write("Goal_Position", [500, 1000, 0, 1000, 100, 950, 100, 100, 1000, 150, 200, 200, 0, 200, 100, 700])
# time.sleep(1)
# for i in range(3):
# robot.hand_bus.write("Goal_Position", [500, 1000-150, 0+250, 1000-150,
# 100+300, 950-250, 100+250,
# 100+200, 1000-300, 150+300,
# 200+500, 200+200, 0+200,
# 200+300, 100+200, 700-200])
# time.sleep(1)
camera = 0
score_threshold = 0.95
iou_threshold = 0.3
app = MediaPipeHandApp(MediaPipeHand.from_pretrained(), score_threshold, iou_threshold)
def frame_processor(frame: np.ndarray) -> np.ndarray:
# Input Prep
NHWC_int_numpy_frames, NCHW_fp32_torch_frames = app_to_net_image_inputs(frame)
# Run Bounding Box & Keypoint Detector
batched_selected_boxes, batched_selected_keypoints = app._run_box_detector(NCHW_fp32_torch_frames)
# The region of interest ( bounding box of 4 (x, y) corners).
# list[torch.Tensor(shape=[Num Boxes, 4, 2])],
# where 2 == (x, y)
#
# A list element will be None if there is no selected ROI.
batched_roi_4corners = app._compute_object_roi(batched_selected_boxes, batched_selected_keypoints)
# selected landmarks for the ROI (if any)
# list[torch.Tensor(shape=[Num Selected Landmarks, K, 3])],
# where K == number of landmark keypoints, 3 == (x, y, confidence)
#
# A list element will be None if there is no ROI.
landmarks_out = app._run_landmark_detector(NHWC_int_numpy_frames, batched_roi_4corners)
app._draw_predictions(
NHWC_int_numpy_frames,
batched_selected_boxes,
batched_selected_keypoints,
batched_roi_4corners,
*landmarks_out,
)
return NHWC_int_numpy_frames[0]
capture_and_display_processed_frames(frame_processor, "QAIHM Mediapipe Hand Demo", camera)
if __name__ == "__main__":
main()

133
examples/test2.py Normal file
View File

@ -0,0 +1,133 @@
#!/usr/bin/env python
#
# ********* Ping Example *********
#
#
# Available SCServo model on this example : All models using Protocol SCS
# This example is tested with a SCServo(STS/SMS/SCS), and an URT
# Be sure that SCServo(STS/SMS/SCS) properties are already set as %% ID : 1 / Baudnum : 6 (Baudrate : 1000000)
#
import os
if os.name == "nt":
import msvcrt
def getch():
return msvcrt.getch().decode()
else:
import sys
import termios
import tty
fd = sys.stdin.fileno()
old_settings = termios.tcgetattr(fd)
def getch():
try:
tty.setraw(sys.stdin.fileno())
ch = sys.stdin.read(1)
finally:
termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
return ch
from scservo_sdk import * # Uses SCServo SDK library
# Default setting
SCS_ID = 1 # SCServo ID : 1
BAUDRATE = 1000000 # SCServo default baudrate : 1000000
DEVICENAME = "/dev/tty.usbserial-2130" # Check which port is being used on your controller
# ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*"
protocol_end = 1 # SCServo bit end(STS/SMS=0, SCS=1)
# Initialize PortHandler instance
# Set the port path
# Get methods and members of PortHandlerLinux or PortHandlerWindows
portHandler = PortHandler(DEVICENAME)
# Initialize PacketHandler instance
# Get methods and members of Protocol
packetHandler = PacketHandler(protocol_end)
# Open port
if portHandler.openPort():
print("Succeeded to open the port")
else:
print("Failed to open the port")
print("Press any key to terminate...")
getch()
quit()
# Set port baudrate
if portHandler.setBaudRate(BAUDRATE):
print("Succeeded to change the baudrate")
else:
print("Failed to change the baudrate")
print("Press any key to terminate...")
getch()
quit()
# Try to ping the SCServo
# Get SCServo model number
scs_model_number, scs_comm_result, scs_error = packetHandler.ping(portHandler, SCS_ID)
if scs_comm_result != COMM_SUCCESS:
print("%s" % packetHandler.getTxRxResult(scs_comm_result))
elif scs_error != 0:
print("%s" % packetHandler.getRxPacketError(scs_error))
else:
print("[ID:%03d] ping Succeeded. SCServo model number : %d" % (SCS_ID, scs_model_number))
ADDR_SCS_PRESENT_POSITION = 56
scs_present_position, scs_comm_result, scs_error = packetHandler.read2ByteTxRx(
portHandler, SCS_ID, ADDR_SCS_PRESENT_POSITION
)
if scs_comm_result != COMM_SUCCESS:
print(packetHandler.getTxRxResult(scs_comm_result))
elif scs_error != 0:
print(packetHandler.getRxPacketError(scs_error))
breakpoint()
scs_present_position = SCS_LOWORD(scs_present_position)
# scs_present_speed = SCS_HIWORD(scs_present_position_speed)
# print("[ID:%03d] PresPos:%03d PresSpd:%03d" % (SCS_ID, scs_present_position, SCS_TOHOST(scs_present_speed, 15)))
print("[ID:%03d] PresPos:%03d" % (SCS_ID, scs_present_position))
groupSyncRead = GroupSyncRead(portHandler, packetHandler, ADDR_SCS_PRESENT_POSITION, 2)
scs_addparam_result = groupSyncRead.addParam(SCS_ID)
if scs_addparam_result != True:
print("[ID:%03d] groupSyncRead addparam failed" % SCS_ID)
quit()
# Syncread present position
scs_comm_result = groupSyncRead.txRxPacket()
if scs_comm_result != COMM_SUCCESS:
print("%s" % packetHandler.getTxRxResult(scs_comm_result))
# Check if groupsyncread data of SCServo#1 is available
scs_getdata_result = groupSyncRead.isAvailable(SCS_ID, ADDR_SCS_PRESENT_POSITION, 2)
if scs_getdata_result == True:
# Get SCServo#1 present position value
scs_present_position = groupSyncRead.getData(SCS_ID, ADDR_SCS_PRESENT_POSITION, 2)
else:
scs_present_position = 0
print("[ID:%03d] groupSyncRead getdata failed" % SCS_ID)
# # Check if groupsyncread data of SCServo#2 is available
# scs_getdata_result = groupSyncRead.isAvailable(SCS2_ID, ADDR_SCS_PRESENT_POSITION, 2)
# if scs_getdata_result == True:
# # Get SCServo#2 present position value
# scs2_present_position_speed = groupSyncRead.getData(SCS2_ID, ADDR_SCS_PRESENT_POSITION, 2)
# else:
# print("[ID:%03d] groupSyncRead getdata failed" % SCS2_ID)
scs_present_position = SCS_LOWORD(scs_present_position)
print("[ID:%03d] PresPos:%03d" % (SCS_ID, scs_present_position))
# Close port
portHandler.closePort()

125
lerobot/common/robot_devices/motors/feetech.py
View File

@ -37,7 +37,7 @@ 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 = {
STS_SERIES_CONTROL_TABLE = {
"Model": (3, 2),
"ID": (5, 1),
"Baud_Rate": (6, 1),
@ -87,6 +87,70 @@ SCS_SERIES_CONTROL_TABLE = {
"Maximum_Acceleration": (85, 2),
}
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": (37, 1),
"Protection_Time": (38, 1),
"Torque_Enable": (40, 1),
"Acceleration": (41, 1),
"Goal_Position": (42, 2),
"Running_Time": (44, 2),
"Goal_Speed": (46, 2),
"Lock": (48, 1),
"Present_Position": (56, 2),
"Present_Speed": (58, 2),
"Present_Load": (60, 2),
"Present_Voltage": (62, 1),
"Present_Temperature": (63, 1),
"Sync_Write_Flag": (64, 1),
"Status": (65, 1),
"Moving": (66, 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),
# "Acceleration": (41, 1),
# "Goal_Time": (44, 2),
# "Torque_Limit": (48, 2),
# "Present_Current": (69, 2),
# # Not in the Memory Table
# "Maximum_Acceleration": (85, 2),
}
STS_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,
}
SCS_SERIES_BAUDRATE_TABLE = {
0: 1_000_000,
1: 500_000,
@ -103,22 +167,31 @@ CONVERT_UINT32_TO_INT32_REQUIRED = ["Goal_Position", "Present_Position"]
MODEL_CONTROL_TABLE = {
"sts_series": STS_SERIES_CONTROL_TABLE,
"scs_series": SCS_SERIES_CONTROL_TABLE,
"sts3215": SCS_SERIES_CONTROL_TABLE,
"sts3215": STS_SERIES_CONTROL_TABLE,
"sts3250": STS_SERIES_CONTROL_TABLE,
"scs0009": SCS_SERIES_CONTROL_TABLE,
}
MODEL_RESOLUTION = {
"scs_series": 4096,
"sts_series": 4096,
"scs_series": 1024,
"sts3215": 4096,
"sts3250": 4096,
"scs0009": 1024,
}
MODEL_BAUDRATE_TABLE = {
"sts_series": STS_SERIES_BAUDRATE_TABLE,
"scs_series": SCS_SERIES_BAUDRATE_TABLE,
"sts3215": SCS_SERIES_BAUDRATE_TABLE,
"sts3215": STS_SERIES_BAUDRATE_TABLE,
"sts3250": STS_SERIES_BAUDRATE_TABLE,
"scs0009": SCS_SERIES_BAUDRATE_TABLE,
}
# High number of retries is needed for feetech compared to dynamixel motors.
NUM_READ_RETRY = 20
NUM_READ_RETRY = 50
NUM_WRITE_RETRY = 20
@ -273,12 +346,18 @@ class FeetechMotorsBus:
self,
port: str,
motors: dict[str, tuple[int, str]],
group_sync_read: bool = True,
group_sync_write: bool = True,
protocol_version: int = 0,
extra_model_control_table: dict[str, list[tuple]] | None = None,
extra_model_resolution: dict[str, int] | None = None,
mock=False,
):
self.port = port
self.motors = motors
self.group_sync_read = group_sync_read
self.group_sync_write = group_sync_write
self.protocol_version = protocol_version
self.mock = mock
self.model_ctrl_table = deepcopy(MODEL_CONTROL_TABLE)
@ -311,7 +390,7 @@ class FeetechMotorsBus:
import scservo_sdk as scs
self.port_handler = scs.PortHandler(self.port)
self.packet_handler = scs.PacketHandler(PROTOCOL_VERSION)
self.packet_handler = scs.PacketHandler(self.protocol_version)
try:
if not self.port_handler.openPort():
@ -335,7 +414,7 @@ class FeetechMotorsBus:
import scservo_sdk as scs
self.port_handler = scs.PortHandler(self.port)
self.packet_handler = scs.PacketHandler(PROTOCOL_VERSION)
self.packet_handler = scs.PacketHandler(self.protocol_version)
if not self.port_handler.openPort():
raise OSError(f"Failed to open port '{self.port}'.")
@ -661,6 +740,8 @@ class FeetechMotorsBus:
else:
import scservo_sdk as scs
scs.SCS_SETEND(self.protocol_version)
return_list = True
if not isinstance(motor_ids, list):
return_list = False
@ -699,6 +780,8 @@ class FeetechMotorsBus:
else:
import scservo_sdk as scs
scs.SCS_SETEND(self.protocol_version)
if not self.is_connected:
raise RobotDeviceNotConnectedError(
f"FeetechMotorsBus({self.port}) is not connected. You need to run `motors_bus.connect()`."
@ -721,6 +804,8 @@ class FeetechMotorsBus:
assert_same_address(self.model_ctrl_table, models, data_name)
addr, bytes = self.model_ctrl_table[model][data_name]
if self.group_sync_read:
group_key = get_group_sync_key(data_name, motor_names)
if data_name not in self.group_readers:
@ -746,6 +831,24 @@ class FeetechMotorsBus:
for idx in motor_ids:
value = self.group_readers[group_key].getData(idx, addr, bytes)
values.append(value)
else:
values = []
for idx in motor_ids:
if bytes == 1:
value, comm, error = self.packet_handler.read1ByteTxRx(self.port_handler, idx, addr)
elif bytes == 2:
value, comm, error = self.packet_handler.read2ByteTxRx(self.port_handler, idx, addr)
elif bytes == 4:
value, comm, error = self.packet_handler.read4ByteTxRx(self.port_handler, idx, addr)
else:
raise ValueError(bytes)
if comm != scs.COMM_SUCCESS:
raise ConnectionError(self.packet_handler.getTxRxResult(comm))
elif error != 0:
raise ConnectionError(self.packet_handler.getRxPacketError(error))
values.append(value)
values = np.array(values)
@ -775,6 +878,8 @@ class FeetechMotorsBus:
else:
import scservo_sdk as scs
scs.SCS_SETEND(self.protocol_version)
if not isinstance(motor_ids, list):
motor_ids = [motor_ids]
if not isinstance(values, list):
@ -811,6 +916,8 @@ class FeetechMotorsBus:
else:
import scservo_sdk as scs
scs.SCS_SETEND(self.protocol_version)
if motor_names is None:
motor_names = self.motor_names
@ -836,6 +943,8 @@ class FeetechMotorsBus:
assert_same_address(self.model_ctrl_table, models, data_name)
addr, bytes = self.model_ctrl_table[model][data_name]
if self.group_sync_write:
group_key = get_group_sync_key(data_name, motor_names)
init_group = data_name not in self.group_readers
@ -857,6 +966,8 @@ class FeetechMotorsBus:
f"Write failed due to communication error on port {self.port} for group_key {group_key}: "
f"{self.packet_handler.getTxRxResult(comm)}"
)
else:
raise NotImplementedError()
# 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)

73
lerobot/configs/robot/hopejr.yaml Normal file
View File

@ -0,0 +1,73 @@
# [SO-100 robot arm](https://github.com/TheRobotStudio/SO-ARM100)
# Requires installing extras packages
# With pip: `pip install -e ".[feetech]"`
# With poetry: `poetry install --sync --extras "feetech"`
# See [tutorial](https://github.com/huggingface/lerobot/blob/main/examples/10_use_so100.md)
_target_: lerobot.common.robot_devices.robots.manipulator.ManipulatorRobot
robot_type: hopejr
calibration_dir: .cache/calibration/hopejr
# `max_relative_target` limits the magnitude of the relative positional target vector for safety purposes.
# Set this to a positive scalar to have the same value for all motors, or a list that is the same length as
# the number of motors in your follower arms.
max_relative_target: null
# leader_arms:
# main:
# _target_: lerobot.common.robot_devices.motors.feetech.FeetechMotorsBus
# port: /dev/tty.usbmodem585A0077581
# motors:
# # name: (index, model)
# shoulder_pan: [1, "sts3215"]
# shoulder_lift: [2, "sts3215"]
# elbow_flex: [3, "sts3215"]
# wrist_flex: [4, "sts3215"]
# wrist_roll: [5, "sts3215"]
# gripper: [6, "sts3215"]
follower_arms:
main:
_target_: lerobot.common.robot_devices.motors.feetech.FeetechMotorsBus
port: /dev/tty.usbserial-2130
motors:
# name: (index, model)
shoulder_pitch: [1, "sts3250"]
shoulder_yaw: [2, "sts3215"] # TODO: sts3250
shoulder_roll: [3, "sts3215"] # TODO: sts3250
elbow_flex: [4, "sts3250"]
wrist_roll: [5, "sts3215"]
wrist_yaw: [6, "sts3215"]
wrist_pitch: [7, "sts3215"]
thumb_basel_rotation: [30, "scs0009"]
thumb_flexion: [27, "scs0009"]
thumb_pinky_side: [26, "scs0009"]
thumb_thumb_side: [28, "scs0009"]
index_flexor: [25, "scs0009"]
index_pinky_side: [31, "scs0009"]
index_thumb_side: [32, "scs0009"]
middle_flexor: [24, "scs0009"]
middle_pinky_side: [33, "scs0009"]
middle_thumb_side: [34, "scs0009"]
ring_flexor: [21, "scs0009"]
ring_pinky_side: [36, "scs0009"]
ring_thumb_side: [35, "scs0009"]
pinky_flexor: [23, "scs0009"]
pinky_pinky_side: [38, "scs0009"]
pinky_thumb_side: [37, "scs0009"]
cameras:
laptop:
_target_: lerobot.common.robot_devices.cameras.opencv.OpenCVCamera
camera_index: 0
fps: 30
width: 640
height: 480
phone:
_target_: lerobot.common.robot_devices.cameras.opencv.OpenCVCamera
camera_index: 1
fps: 30
width: 640
height: 480

4
tests/mock_scservo_sdk.py
View File

@ -18,6 +18,10 @@ def convert_to_bytes(value, bytes):
return value
def SCS_SETEND(protocol_version):
del protocol_version
def get_default_motor_values(motor_index):
return {
# Key (int) are from SCS_SERIES_CONTROL_TABLE