WIP

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()

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()

lerobot/common/robot_devices/motors/feetech.py

@@ -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,31 +804,51 @@ class FeetechMotorsBus:
 
         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:
+        if self.group_sync_read:
-            # create new group reader
+            group_key = get_group_sync_key(data_name, motor_names)
-            self.group_readers[group_key] = scs.GroupSyncRead(
+
-                self.port_handler, self.packet_handler, addr, bytes
+            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)}"
+                )
+
+            values = []
             for idx in motor_ids:
-                self.group_readers[group_key].addParam(idx)
+                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)
 
-        for _ in range(NUM_READ_RETRY):
+                if comm != scs.COMM_SUCCESS:
-            comm = self.group_readers[group_key].txRxPacket()
+                    raise ConnectionError(self.packet_handler.getTxRxResult(comm))
-            if comm == scs.COMM_SUCCESS:
+                elif error != 0:
-                break
+                    raise ConnectionError(self.packet_handler.getRxPacketError(error))
 
-        if comm != scs.COMM_SUCCESS:
+                values.append(value)
-            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)
 
@@ -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,27 +943,31 @@ class FeetechMotorsBus:
 
         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 self.group_sync_write:
-        if init_group:
+            group_key = get_group_sync_key(data_name, motor_names)
-            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):
+            init_group = data_name not in self.group_readers
-            data = convert_to_bytes(value, bytes, self.mock)
             if init_group:
-                self.group_writers[group_key].addParam(idx, data)
+                self.group_writers[group_key] = scs.GroupSyncWrite(
-            else:
+                    self.port_handler, self.packet_handler, addr, bytes
-                self.group_writers[group_key].changeParam(idx, data)
+                )
 
-        comm = self.group_writers[group_key].txPacket()
+            for idx, value in zip(motor_ids, values, strict=True):
-        if comm != scs.COMM_SUCCESS:
+                data = convert_to_bytes(value, bytes, self.mock)
-            raise ConnectionError(
+                if init_group:
-                f"Write failed due to communication error on port {self.port} for group_key {group_key}: "
+                    self.group_writers[group_key].addParam(idx, data)
-                f"{self.packet_handler.getTxRxResult(comm)}"
+                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)

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

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