Simple class to control the LewanSoul XArm over the USBHID interface

xarm_control.py

"""
sudo apt-get install libhidapi-hidraw0 libhidapi-libusb0
pip3 install --user hid

Notes:
- servos_off will power off the servos
- sending a movement command wakes up unpowered servos
- position readouts are sadly pretty slow, they can take up to 450ms
"""

import time
import easyhid
import numpy as np

def itos(v):
    lsb = v & 0xFF
    msb = v >> 8
    return lsb, msb

class XArm():
    def __init__(self, pid=22352):

        # Stores an enumeration of all the connected USB HID devices
        en = easyhid.Enumeration()

        # return a list of devices based on the search parameters
        devices = en.find(vid=1155, pid=pid)

        # print a description of the devices found
        #for dev in devices:
        #    print(dev.description())

        assert len(devices) > 0
        self.dev = devices[0]

        # open a device
        self.dev.open()
        print('Connected to xArm device')

    def __del__(self):
        print('Closing xArm device')
        self.dev.close()

    def move_to(self, id, pos, time=0):
        """
        CMD_SERVO_MOVE
        0x55 0x55 len 0x03 count [time_lsb time_msb, id, pos_lsb pos_msb]
        Servo position is in range [0, 1000]
        """

        t_lsb, t_msb = itos(time)
        p_lsb, p_msb = itos(pos)
        self.dev.write([0x55, 0x55, 8, 0x03, 1, t_lsb, t_msb, id, p_lsb, p_msb])

    def move_all(self, poss, time=0):
        """
        Set the position of all servos at once
        """

        for i in range(6):
            self.move_to(id=i+1, pos=poss[i], time=time)

    def servos_off(self):
        self.dev.write([0x55, 0x55, 9, 20, 6, 1, 2, 3, 4, 5, 6])

    def read_pos(self):
        """
        Read the position of all 6 servos
        ServoPositionRead 21 (byte)count { (byte)id }; (byte)count { (byte)id (ushort)position }
        """

        self.dev.write([
            0x55, 0x55,
            9,  # Len
            21, # Cmd
            6,  # Count
            1,
            2,
            3,
            4,
            5,
            6
        ])

        ret = self.dev.read()
        count = ret[4]
        assert count == 6

        poss = []
        for i in range(6):
            id = ret[5 + 3*i]
            p_lsb = ret[5 + 3*i + 1]
            p_msb = ret[5 + 3*i + 2]
            pos = (p_msb << 8) + p_lsb
            poss.append(pos)

        return np.array(poss)

    def rest(self):
        self.move_all([500, 500, 200, 900, 800, 500], time=1500)
        time.sleep(2)
        self.servos_off()


class SafeXArm:
    """
    Wrapper to limit motion range and speed to maximize durability
    Also remaps joint angles into the [-1, 1] range
    """

    def __init__(self, **kwargs):
        self.arm = XArm(**kwargs)

        self.min_pos = np.array([
            100, # Base
            200,
            400,
            100,
            50,  # Wrist
            200, # Gripper
        ])

        self.max_pos = np.array([
            900, # Base
            800,
            900,
            600,
            850,  # Wrist
            650,  # Gripper
        ])

        # Maximum movement speed in (range/second)
        self.max_speed = 250

        self.move_all([0] * 6)
        time.sleep(2)

    def read_pos(self):
        return np.flip(self.arm.read_pos(), 0)

    def rest(self):
        return self.arm.rest()

    def move_all(self, pos):
        if not isinstance(pos, np.ndarray):
            pos = np.array(pos)

        # [-1, 1] => [0, 1]
        pos = (pos + 1) / 2
        target = self.min_pos + pos * (self.max_pos - self.min_pos)

        target = np.flip(target, 0).astype(np.uint16)

        # TODO: compute time needed based on last position
        # Compute time needed to move each joint to target given max_speed
        #cur_pos = self.arm.read_pos()
        #time = (abs(cur_pos - target) / self.max_speed)
        #time = (time * 1000).astype(np.uint16)

        for i in range(6):
            self.arm.move_to(id=i+1, pos=target[i], time=100)

def demo():
    arm = SafeXArm()

    # To the right
    arm.move_all([-1, 0, 0, 0, 0, 0])
    time.sleep(2)

    # To the left
    arm.move_all([1, 0, 0, 0, 0, 0])
    time.sleep(2)

    # Default position
    arm.move_all([0, 0, 0, 0, 0, 0])
    time.sleep(2)

    # Open gripper
    arm.move_all([0, 0, 0, 0, 0, -1])
    time.sleep(2)

    # Close gripper
    arm.move_all([0, 0, 0, 0, 0, 1])
    time.sleep(2)

    # Put the arm back in a resting position
    arm.rest()

demo()

I think there's a mistake in the comment on line 47, the actual data sent includes an extra 1 byte before time_lsb; according to https://github.com/ccourson/LewanSoul-xArm this is the count parameter.

Updated the comment 👍