diff --git a/examples/test.py b/examples/test.py new file mode 100644 index 000000000..1e10e6c72 --- /dev/null +++ b/examples/test.py @@ -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() diff --git a/examples/test2.py b/examples/test2.py new file mode 100644 index 000000000..3c5072bdf --- /dev/null +++ b/examples/test2.py @@ -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() diff --git a/lerobot/common/robot_devices/motors/feetech.py b/lerobot/common/robot_devices/motors/feetech.py index 0d5480f7a..8eb4bd6dc 100644 --- a/lerobot/common/robot_devices/motors/feetech.py +++ b/lerobot/common/robot_devices/motors/feetech.py @@ -37,7 +37,7 @@ # 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 @@ "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 @@ 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 @@ def __init__( 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 @@ def connect(self): 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 @@ def reconnect(self): 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 @@ def read_with_motor_ids(self, motor_models, motor_ids, data_name, num_retry=NUM_ 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 @@ def read(self, data_name, motor_names: str | list[str] | None = None): 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,31 +804,51 @@ def read(self, data_name, motor_names: str | list[str] | None = None): 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: - # 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) + if self.group_sync_read: + group_key = get_group_sync_key(data_name, motor_names) - for _ in range(NUM_READ_RETRY): - comm = self.group_readers[group_key].txRxPacket() - if comm == scs.COMM_SUCCESS: - break + if data_name not in self.group_readers: + # 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)}" + ) - 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) + 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) - values = [] - for idx in motor_ids: - value = self.group_readers[group_key].getData(idx, addr, bytes) - values.append(value) + 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 @@ def write_with_motor_ids(self, motor_models, motor_ids, data_name, values, num_r 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 @@ def write(self, data_name, values: int | float | np.ndarray, motor_names: str | else: import scservo_sdk as scs + scs.SCS_SETEND(self.protocol_version) + if motor_names is None: motor_names = self.motor_names @@ -836,27 +943,31 @@ def write(self, data_name, values: int | float | np.ndarray, motor_names: str | 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 - ) + if self.group_sync_write: + group_key = get_group_sync_key(data_name, motor_names) - for idx, value in zip(motor_ids, values, strict=True): - data = convert_to_bytes(value, bytes, self.mock) + init_group = data_name not in self.group_readers if init_group: - self.group_writers[group_key].addParam(idx, data) - else: - self.group_writers[group_key].changeParam(idx, data) + self.group_writers[group_key] = scs.GroupSyncWrite( + self.port_handler, self.packet_handler, addr, bytes + ) - 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)}" - ) + 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)}" + ) + 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) diff --git a/lerobot/configs/robot/hopejr.yaml b/lerobot/configs/robot/hopejr.yaml new file mode 100644 index 000000000..16a677cd8 --- /dev/null +++ b/lerobot/configs/robot/hopejr.yaml @@ -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 diff --git a/tests/mock_scservo_sdk.py b/tests/mock_scservo_sdk.py index 596978c00..97b140fb0 100644 --- a/tests/mock_scservo_sdk.py +++ b/tests/mock_scservo_sdk.py @@ -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