复刻低成本机械臂 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 FeetechMotorsBusclass 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_idself.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 = 600loop_count = 10result = 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()