[Humanoid] Dynamixel SDK를 이용한 AX-12A Control Class

Godssi·2024년 11월 9일

Dynamixel Shield ROS 2 dynamixel python u2d2

Humanoid

목록 보기

3/3

Arduino Shield를 이용하여 Dynamixel을 구동했을 때 생기는 Debugging Error에 대해서 알아보고, Dynamixel SDK를 이용하여 ROS 2 AX-12A Control 패키지를 만들어 보려고 한다.

Dynamixel Shield Debugging Error

이전 글 참고([Humanoid] Dynamixel AX-12A 동시 구동)
Dynamixel Shield를 Arduino Uno 혹은 Mega에서 사용할 경우, SoftwareSerial 라이브러리를 사용한다.

SoftwareSerial Library

일반 입출력 데이터 핀을 RX, TX 핀으로 동작할 수 있게 해주는 라이브러리로 이를 통해 RX, TX로 작동하게 된 핀을 소프트웨어 시리얼이라고 한다.
그와 반대로 일반적으로 지정되어 있는 예를 들어 Arduino Uno의 0,1번 핀과 같이 본래 RX, TX 핀을 Hardware Serial이라고 한다.

보드 종류에 따른 사용 가능한 핀

Arduino Uno(ATmega328) : 모든 핀 가능, 0번, 1번 핀(HardwareSerial)은 충돌 회피
Arduino Mega(ATmega2560) : 10~15번, 50~53번, 62~69번, Mega의 경우 HardwareSerial이 3개이므로 잘 사용 X
Arduino Leonardo : 대부분의 디지털 핀 그러나 8~11번, 14~16번 핀만 RX로 사용 가능, Serial1을 제공하므로 SoftwareSerial 잘 사용 X
ESP8266 : 일반적으로 D1, D2, D5, D6, D7 핀에서 SoftwareSerial 사용 가능
ESP32 : 여러 개의 HardwareSerial 포트를 지원하므로 SoftwareSerial 사용 X

단점

속도 제한 : SoftwareSerial의 통신 속도는 하드웨어 시리얼보다 낮다. 일반적으로 9600bps에서는 안정저긍로 동작하지만, 그 이상의 속도에서는 데이터 누락, 손실 등의 문제 발생
디지털 핀 차지 : 디지털 핀을 차지하게 되므로, 다른 기능에 사용할 수 있는 핀이 줄어드는 문제
메모리 사용 : 소프트웨어적으로 시리얼 통신을 구현하기 때문에 라이브러리와 데이터 버퍼가 메모리를 추가로 점유, 특히 작은 메모리를 가진 Arduino Uno, Nano 등의 보드에서는 메모리 부족 문제 발생 가능

위의 단점 중 속도 제한으로 인한 데이터 누락으로 인해 앞선 Arduino Dynamixel Shield의 Debugging 오류가 생겼던 것이다.

따라서 해당 문제를 해결하기 위해서는 Controller 교체 혹은 PC로 직접 제어 중에 한 가지 방법을 사용해야 한다.

ROS2 Dynamixel SDK 개선

Arduino Shield Debugging Error에 대한 회의를 끝낸 결과, PC로 직접 Dynamixel을 제어하는 방법을 사용하기로 했다.
그에 따라 기존의 ROS 2 Dynamixel SDK를 개선하는 작업을 진행하려고 한다.

개선 방향

기존의 Dynamixel SDK는 통신에 대한 API로 이루어져 있다.
그래서 일반적인 사용자들이 해당 코드를 접하게 될 경우, 코드를 이해하는데 어려움을 겪을 수 있다.
그에 따라, 통신으로 이루어진 API를 직관적인 API를 가지고 있는 HumanoidSDK라는 Class를 만드는 방향으로 개선하려고 한다.

제공되는 Class API

Setup 및 Close

DynamixelSetup : 특정 ID의 Dynamixel 한 개를 Setup
- API Syntax
```
DynamixelSetup(id)
```
- Parameters
  1. id : Dynamixel ID 값
MultiDynamixelSetup : 여러 개의 Dynamixel를 순차적으로 Setup
- API Syntax
```
MultiDynamixelSetup(ids)
```
- Parameters
  1. ids : 여러 개의 Dynamixel ID 값을 담고 있는 List
DynamixelClose : 특정 ID의 Dynamixel 한 개를 Close
- API Syntax
```
DynamixelClose(id)
```
- Parameters
  1. id : Dynamixel ID 값
MultiDynamixelClose : 여러 개의 Dynamixel를 순차적으로 Close
- API Syntax
```
MultiDynamixelClose(ids)
```
- Parameters
  1. ids : 여러 개의 Dynamixel ID 값을 담고 있는 List

Control

DynamixelWrite
- API Syntax
```
DynamixelWrite(id, position)
```
- Parameters
  1. id : Dynamixel ID 값
  2. position : Dynamixel 목표 위치 Tick 값
MultiDynamixelWrite
- API Syntax
```
MultiDynamixelWrite(ids, positions)
```
- Parameters
  1. ids : 여러 개의 Dynamixel ID 값을 담고 있는 List
  2. positions: 여러 개의 Dynamixel 목표 위치 Tick 값을 담고 있는 List
DynamixelRead
- API Syntax
```
DynamixelRead(id)
```
- Parameters
  1. id : Dynamixel ID 값
MultiDynamixelRead
- API Syntax
```
MultiDynamixelRead(ids)
```
- Parameters
  1. ids : 여러 개의 Dynamixel ID 값을 담고 있는 List

전체 코드

예제 코드

import os

if os.name == 'nt':
    import msvcrt
    def getch():
        return msvcrt.getch().decode()
else:
    import sys, tty, termios
    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 dynamixel_sdk import *  
    
# Control table address AX-12A
ADDR_AX_TORQUE_ENABLE      = 24
ADDR_AX_GOAL_POSITION      = 30
ADDR_AX_PRESENT_POSITION   = 36

# Protocol version
PROTOCOL_VERSION            = 1.0               # See which protocol version is used in the Dynamixel

# Default setting
BAUDRATE                    = 1000000           # Dynamixel default baudrate : 57600
DEVICENAME                  = '/dev/ttyUSB0'    # Check which port is being used on your controller

TORQUE_DISABLE              = 0                 # Value for disabling the torque
TORQUE_ENABLE               = 1                 # Value for enabling the torque
DXL_MINIMUM_POSITION_VALUE  = 0                 # Dynamixel will rotate between this value
DXL_MAXIMUM_POSITION_VALUE  = 1023              # and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.)
DXL_MOVING_STATUS_THRESHOLD = 20                # Dynamixel moving status threshold

PortHandler = PortHandler(DEVICENAME)
PacketHandler = PacketHandler(PROTOCOL_VERSION)

index = 0
dxl_goal_position = [[512, 512], [0, 0]]         # Goal position

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

def DynamixelSetup(id):
    dxl_comm_result, dxl_error = PacketHandler.write1ByteTxRx(PortHandler, id, ADDR_AX_TORQUE_ENABLE, TORQUE_ENABLE)
    if dxl_comm_result != COMM_SUCCESS:
        print("%s" % PacketHandler.getTxRxResult(dxl_comm_result))
    elif dxl_error != 0:
        print("%s" % PacketHandler.getRxPacketError(dxl_error))
    else:
        print("Dynamixel has been successfully connected")

def MultiDynamixelSetup(ids):
    for id in ids:
        DynamixelSetup(id)

def DynamixelClose(id):
    dxl_comm_result, dxl_error = PacketHandler.write1ByteTxRx(PortHandler, id, ADDR_AX_TORQUE_ENABLE, TORQUE_DISABLE)
    if dxl_comm_result != COMM_SUCCESS:
        print("%s" % PacketHandler.getTxRxResult(dxl_comm_result))
    elif dxl_error != 0:
        print("%s" % PacketHandler.getRxPacketError(dxl_error))
    else:
        print("Dynamixel has been successfully disconnected")

def MultiDynamixelClose(ids):
    for id in ids:
        DynamixelClose(id)

def DynamixelRead(id):
    dxl_present_position, dxl_comm_result, dxl_error = PacketHandler.read2ByteTxRx(PortHandler, id, ADDR_AX_PRESENT_POSITION)
    if dxl_comm_result != COMM_SUCCESS:
        print("%s" % PacketHandler.getTxRxResult(dxl_comm_result))
    elif dxl_error != 0:
        print("%s" % PacketHandler.getRxPacketError(dxl_error))
    else:
        print("Dynamixel present position is: %d" % dxl_present_position)

def MultiDynamixelRead(ids):
    for id in ids:
        DynamixelRead(id) 

def DynamixelWrite(id, position):
    dxl_comm_result, dxl_error = PacketHandler.write2ByteTxRx(PortHandler, id, ADDR_AX_GOAL_POSITION, position)
    if dxl_comm_result != COMM_SUCCESS:
        print("%s" % PacketHandler.getTxRxResult(dxl_comm_result))
    elif dxl_error != 0:
        print("%s" % PacketHandler.getRxPacketError(dxl_error))
    else:
        print("Dynamixel has been successfully connected")

def MultiDynamixelWrite(ids, positions):
    for id, position in zip(ids, positions):
        DynamixelWrite(id, position)

# Enable Dynamixel Torque for all motors
MultiDynamixelSetup([11, 17])

# Read present position for all motors
MultiDynamixelRead([11, 17])

while 1:
    print("Press any key to continue! (or press ESC to quit!)")
    if getch() == chr(0x1b):
        break

    # Write goal position for all motors
    MultiDynamixelWrite([11, 17], dxl_goal_position[index])

    while 1:
        # Read present position for all motors
        time.sleep(1)
        MultiDynamixelRead([11, 17])
        break

    if index == 0:
        index = 1
    else:
        index = 0

# Disable Dynamixel Torque for all motors
MultiDynamixelClose([11, 17])

# Close port
PortHandler.closePort()

시연 영상

개선 및 추가할 사항

Write API 개선 필요 : position값까지 이동했는지 확인하면서 해당 값까지 이동했을 때, 멈추게 개선
Angle 제어 : 현재는 Motor의 Tick 값(Encoder 값)을 이용해서 위치 제어를 진행하지만, Angle 값을 통해서 제어할 수 있게 개선 필요
ROS 2 변환 필요 : ROS 2에서 사용할 수 있게 패키지 변환 필요
- Custom Message(Topic, Service) 제작 필요