复刻低成本机械臂 SO-ARM100 单关节控制（附代码）

视频讲解：

代码仓库：GitHub - LitchiCheng/SO-ARM100: Some Test code on SO-ARM100

昨天用bambot的web的方式调试了整个机械臂，对于后面的仿真的sim2real来说，还是需要单独封装好这些控制，方便后面迁移到其他的测试平台中。

翻了lerobot的代码，可以看到对于feetech舵机的控制等封装已经挺完善，本质就是通过串口和舵机进行协议通信，这个在 lerobot/lerobot/common/robot_devices/motors/feetech.py 中可以看下相应的寄存器定义

接下来我们就针对lerobot这部分代码进行测试，测试单个关节的运动，首先是创建一个FeetechMotor的类，传入port和id就就可以方便的调试某个电机

复制代码

import sys
import os
import time
parent_dir = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
sys.path.append(parent_dir)

from feetech import FeetechMotorsBusConfig
from feetech import FeetechMotorsBus

class FeetechMotor:
    def __init__(self, motor_id, port="/dev/ttyACM0"):
        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"],
        },
        self.motor_id = motor_id
        self.motors_bus = FeetechMotorsBus(FeetechMotorsBusConfig(
            port=port,
            motors=motors,
        ))
        self.motors_bus.connect()

    def setPosition(self, position):
        self.motors_bus.write_with_motor_ids(self.motors_bus.motor_models, self.motor_id, "Goal_Position", position)

    def getPosition(self):
        return self.motors_bus.read_with_motor_ids(self.motors_bus.motor_models, self.motor_id, "Present_Position")

    def close(self):
        self.motors_bus.disconnect()

实验测试目标，让某个关节往复运动，声明一个函数generatePositionSequence用来生成位置的序列

复制代码

def generatePositionSequence(start_position, range_value, loops=1):
    sequence = []
    for _ in range(loops):
        forward_positions = list(range(start_position, start_position + range_value + 1))
        sequence.extend(forward_positions)
        backward_positions = list(range(start_position + range_value - 1, start_position - 1, -1))
        sequence.extend(backward_positions)
    return sequence

执行部分，指定电机id和串口名称，开始让电机运动到2048也就是中间的位置，然后再开始往复运动

复制代码

if __name__ == "__main__":
    motor = FeetechMotor(5, "/dev/ttyACM0")
    motor.setPosition(2048)
    time.sleep(1)
    start_position = motor.getPosition()
    print(f"Start position: {start_position}")
    range_val = 600
    loop_count = 10
    result = generatePositionSequence(start_position, range_val, loop_count)
    for position in result:
        motor.setPosition(position)
        current_position = motor.getPosition()
        print(f"Current position: {current_position}, Goal position: {position}")
        time.sleep(0.005)
    motor.close()