로보티즈의 다이나믹셀 XM 시리즈 Python으로 살짝 구동시켜보기~

바로 얼마전에 로보티즈의 다이나믹셀 이야기를 했는데요. 내침김에 좀 더 찾아보니 Python이나 MATLAB으로 접근할 수 있도록 Software SDK를 배포하고 있더군요. [바로가기]에 가시면 됩니다. 그래서 둘러보고 Python으로 접근해보는 것을 한 번 했지요~^^ 오랜만에 모터를 살짝꿍 돌리니... 참 재미있네요^^

위 사이트가 아까 이야기한 SDk Github입니다. 그냥 편안하게 다운받으시공~

저기 보이는 read_write.py를 기본으로 움직일 생각입니다. 저 파일을 수정하고 터미널에서 python read_write.py라고 입력하면 실행됩니다. 아참 저는 이미 Anaconda3 [바로가기]가 깔려있거든요. 그리고 그 전에 dynamixel_functions.py라는 파일을 찾아서 첫 부분의 dxl_lib의 위치를 지정하는 곳에 자신의 OS에 맞게 지정하시면 됩니다. 

그리고 저렇게 연결해 두는거죠~^^ 오늘은 뭐 실제로 돌아간 영상은 생략해도 될 듯 합니다. 그냥 사진으로 대체~~^^ 아무튼.. read_write.py 파일을 보고 이것 저것 따라한 것을 이야기할려구요...^^

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import os
import msvcrt
import time
import numpy as np
import matplotlib.pyplot as plt

def getch():
    return msvcrt.getch().decode()

os.sys.path.append('../dynamixel_functions_py')             # Path setting

import dynamixel_functions as dynamixel                     # Uses Dynamixel SDK library

# Control table address
ADDR_XM_TORQUE_ENABLE       = 64                          # Control table address is different in Dynamixel model
ADDR_XM_GOAL_POSITION       = 116
ADDR_XM_PRESENT_POSITION    = 132
ADDR_XM_PRESENT_Current     = 126
Goal_Current                = 102
Opoerating_Mode             = 11
Position_P                  = 84

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

# Default setting
DXL_ID                      = 0                             # Dynamixel ID: 1
BAUDRATE                    = 1000000
DEVICENAME                  = "COM3".encode('utf-8')# Check which port is being used on your controller
                                                            # ex) Windows: "COM1"   Linux: "/dev/ttyUSB0"

TORQUE_ENABLE               = 1                             # Value for enabling the torque
TORQUE_DISABLE              = 0                             # Value for disabling the torque

# Initialize PortHandler Structs
# Set the port path
# Get methods and members of PortHandlerLinux or PortHandlerWindows
port_num = dynamixel.portHandler(DEVICENAME)

# Initialize PacketHandler Structs
dynamixel.packetHandler()

index = 0
dxl_goal_position = 0

target_pos = []
current_pos = []
time_axis = []
current_cur = []
pos_error = []

# Open port
if dynamixel.openPort(port_num):
    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 dynamixel.setBaudRate(port_num, BAUDRATE):
    print("Succeeded to change the baudrate!")
else:
    print("Failed to change the baudrate!")
    print("Press any key to terminate...")
    getch()
    quit()

# Set Opoerating Mode 3 : Current Base Position
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, Opoerating_Mode, 5)
print("--> Set Opoerating Mode 3 ")

# Set Goal Current
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, Goal_Current, 100)
print("--> Set Goal Current")

# Set Position P Gain
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, Position_P, 800)
print("--> Set Position P Gain")

# Enable Dynamixel Torque
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_XM_TORQUE_ENABLE, TORQUE_ENABLE)
print("--> Torque Enable")

print("Press space-bar")
dxl_goal_position = 0
dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_XM_GOAL_POSITION, dxl_goal_position)

anykeypress = getch()

start_time = time.time()

while 1:
    get_time = time.time() - start_time

    if get_time < 0.2:
        dxl_goal_position = 4000
    else:
        dxl_goal_position = 0

    dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_XM_GOAL_POSITION, dxl_goal_position)

    position = dynamixel.read4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_XM_PRESENT_POSITION)

    print("Time:%02.4f  GoalPos:%03d  PresPos:%03d" % (get_time, dxl_goal_position, position))

    target_pos.append(dxl_goal_position)
    current_pos.append(position)
    time_axis.append(get_time)

    if get_time > 0.5:
        break

# Disable Dynamixel Torque
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_XM_TORQUE_ENABLE, TORQUE_DISABLE)

# Close port
dynamixel.closePort(port_num)

plt.figure()
plt.plot(time_axis, target_pos, label='target')
plt.plot(time_axis, current_pos, 'ro', label='position')
plt.legend()
plt.show()

먼저 위 코드를 실행해볼 겁니다.. 그전에... 초반에 Control Table Address하는 부분의 이름과 숫자느 모두

여기 [바로가기]에서 가져온 것입니다. 그리고 또 저 표를 보면서 조심해야할 것은 Size인데요.. 바로 Size에 따라 읽고 쓰는 명령에 1Byte, 2Byte, 4Byte.. 이렇게 구분이 가도록 이름이 명명되어 있습니다. 조심해서 사용하야겠죠... 저 코드는 위치 제어 명령을 하달하고, 그 위치에 도달하기 전에 다른 위치명령을 주면 어떻게 되는지 보고 싶어서 구현해 본 것입니다.

결과가 저렇게 됩니다. 0.2초 직전에 위치 명령이 변경되었고... 바로 방향을 바꾸지 못하고 좀 더 진행하다가 0.25초 지점쯤 방향을 바꾸게 되죠. 뭐 정상적입니다.^^. 보통 이게 기구에 무리를 주지 않는 방법이기도 하구요. 그런데.. 제가 못 찾아서 그런지 비상정지 명령이 안보이더라구요.. 토크는 유지하면서 움직임을 갑자기 멈추는 명령이.ㅠㅠ. 뭐 아무튼... 이제 살짝 시간을 이용해서 삼각함수로 구현되도록 코드를 바꾸었습니다.

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import os
import msvcrt
import time
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.animation as animation
from matplotlib import style
style.use('fivethirtyeight')

def getch():
    return msvcrt.getch().decode()

os.sys.path.append('../dynamixel_functions_py')             # Path setting

import dynamixel_functions as dynamixel                     # Uses Dynamixel SDK library

# Control table address
ADDR_XM_TORQUE_ENABLE       = 64                          # Control table address is different in Dynamixel model
ADDR_XM_GOAL_POSITION       = 116
ADDR_XM_PRESENT_POSITION    = 132
ADDR_XM_PRESENT_Current     = 126
ADDR_XM_PRESENT_Velocity    = 128
Goal_Current                = 102
Opoerating_Mode             = 11
Position_P                  = 84
Moving_Threshold            = 122

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

# Default setting
DXL_ID                      = 0                             # Dynamixel ID: 1
BAUDRATE                    = 1000000
DEVICENAME                  = "COM3".encode('utf-8')# Check which port is being used on your controller
                                                            # ex) Windows: "COM1"   Linux: "/dev/ttyUSB0"

TORQUE_ENABLE               = 1                             # Value for enabling the torque
TORQUE_DISABLE              = 0                             # Value for disabling the torque

# Initialize PortHandler Structs
# Set the port path
# Get methods and members of PortHandlerLinux or PortHandlerWindows
port_num = dynamixel.portHandler(DEVICENAME)

# Initialize PacketHandler Structs
dynamixel.packetHandler()

index = 0
dxl_goal_position = 0

Amp = 4096/2
period = 10
freq = 1/period
bias = 4096/2

target_pos = []
current_pos = []
time_axis = []
current_cur = []
pos_error = []

# Open port
if dynamixel.openPort(port_num):
    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 dynamixel.setBaudRate(port_num, BAUDRATE):
    print("Succeeded to change the baudrate!")
else:
    print("Failed to change the baudrate!")
    print("Press any key to terminate...")
    getch()
    quit()

# Set Opoerating Mode 3 : Current Base Position
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, Opoerating_Mode, 5)
print("--> Set Opoerating Mode 3 ")

# Set Goal Current
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, Goal_Current, 100)
print("--> Set Goal Current")

# Set Position P Gain
dynamixel.write2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, Position_P, 800)
print("--> Set Position P Gain")

# Enable Dynamixel Torque
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_XM_TORQUE_ENABLE, TORQUE_ENABLE)
print("--> Torque Enable")

print("Press space-bar")
dxl_goal_position = int(bias)
dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_XM_GOAL_POSITION, dxl_goal_position)

anykeypress = getch()

start_time = time.time()

while 1:
    get_time = time.time() - start_time
    ref_position = Amp * np.sin(2*np.pi*freq*get_time) + bias
    dxl_goal_position = int(ref_position)    

    dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_XM_GOAL_POSITION, dxl_goal_position)

    position = dynamixel.read4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_XM_PRESENT_POSITION)
    current = dynamixel.read2ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_XM_PRESENT_Current)
    current = current if current < 65536/2 else current - 65536

    print("Time:%02.4f  GoalPos:%03d  PresPos:%03d  Current:%03d" % (get_time, dxl_goal_position, position, current))

    target_pos.append(dxl_goal_position)
    current_pos.append(position)
    time_axis.append(get_time)
    current_cur.append(current)
    pos_error.append(dxl_goal_position - position)

    if get_time > period:
        break

# Disable Dynamixel Torque
dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_XM_TORQUE_ENABLE, TORQUE_DISABLE)

# Close port
dynamixel.closePort(port_num)

plt.figure()

plt.subplot(311)
plt.plot(time_axis, target_pos, label='target')
plt.plot(time_axis, current_pos, label='position')
plt.legend(loc='best')

plt.subplot(312)
plt.plot(time_axis, pos_error, label='position error')
plt.legend(loc='best')

plt.subplot(313)
plt.plot(time_axis, current_cur, label='current')
plt.legend(loc='best')

plt.show()

.이렇게 코드를 구현해서.. period를 입력하면 삼각파를 지령으로 움직이도록 했습니다.^^.

이건 Operating Mode를 Current Base Position Control로 두고... 10초 동안 정현파를 움직이라고 둔 것입니다.

요건 그냥 Goal Current 값을 15정도로 낮춰놓고 움직이는 중간에 제가 손으로 잡은 겁니다. 그러니 제일 위의 그래프처럼 지령은 변하는데 위치는 못 변한거죠^^. 이때, Current base control이기 때문에 설정된 15code의 전류는 멈춘동안 유지되는 듯 보입니다. 괜찮네요^^. 아 혹시 Goal Current와 Current Limit을 혼돈하시면 안됩니다^^ 뭐 당연히 그러니까 공개하신거겠지만, Python에서 접근해서 잘 돌려 보았습니다. 이제 이 모터로 저희가 좀 재미있는 걸 해볼려고 하거든요^^. 로봇 만들기 도전할려구요^^