#プログラミング ROS< ps4のコントローラでアームロボット操作 >
はじめに
ROS(Robot Operating System)をさらに扱えるようになることが目的である．その第44弾として，「ps4のコントローラでアームロボット操作」を扱う．
環境
コンピュータ
| デバイス | raspberry pi 4B+ |
| 実装RAM | 4.00 GB (7.89 GB 使用可能) |
| OS | ubuntu-mate-20.04.1-desktop-amd64 |
ROS
| Distribution | noetic |
コントローラ
| メーカー | SONY |
| シリーズ | DUALSHOCK4 Wireless Controller |
Dynamixel
| メーカー | ROBOTIS |
| シリーズ | XM430-W350-R |
| 仕様詳細 | https://emanual.robotis.com/docs/en/dxl/x/xm430-w350/ |
ロボット
| メーカー | ROBOTIS |
| シリーズ | OpenMANIPULATOR-X |
| 仕様詳細 | https://emanual.robotis.com/docs/en/platform/openmanipulator_x/overview/ |
 |
ロボットを動かすために
第42弾ではps4のコントローラ操作について学習した．
第43弾ではDynamixelで構成されたアームロボットの駆動方法を学習した．
今回は，それらの総集編である．ロボットをps4のコントローラで動かすためには，Dynamixelとps4のコントローラをつなぐものが必要となる．なので，流れとしては以下のようになる．
- 扱うdynamixelのポートを認識してトルクオンにするプログラムの改良
- /joyを購読して，その情報をもとに/set_positionを配信するプログラムの作成
- joyと上の2つのプログラム，合計3つを同時に実行するlaunchファイルの作成
これらを順に作成していくこととする．
作業①：dynamixel認識プログラムの改良
第43弾でも示していたとおり，dynamixel_sdk_examplesには，dynamixelと通信するためのプログラムが用意されている．ここでは，編集したいということと変更を簡単に反映させるということを踏まえてpythonで書かれたread_write_node.pyを編集することとする．Pythonのほうが言語として慣れているためである．編集内容は，idを11~15まで適用できるようにすること．
**read_write_node.pyの改変プログラム**
read_write_armbot.py
# !/usr/bin/env python3
# *******************************************************************************
# Copyright 2021 ROBOTIS CO., LTD.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# *******************************************************************************
# *******************************************************************************
# This example is written for DYNAMIXEL X(excluding XL-320) and MX(2.0) series with U2D2.
# For other series, please refer to the product eManual and modify the Control Table addresses and other definitions.
# To test this example, please follow the commands below.
#
# Open terminal #1
# $ roscore
#
# Open terminal #2
# $ rosrun dynamixel_sdk_examples read_write_node.py
#
# Open terminal #3 (run one of below commands at a time)
# $ rostopic pub -1 /set_position dynamixel_sdk_examples/SetPosition "{id: 1, position: 0}"
# $ rostopic pub -1 /set_position dynamixel_sdk_examples/SetPosition "{id: 1, position: 1000}"
# $ rosservice call /get_position "id: 1"
#
# Author: Will Son
# ******************************************************************************/
import os
import rospy
from dynamixel_sdk import *
from dynamixel_sdk_examples.srv import *
from dynamixel_sdk_examples.msg import *
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
# Control table address
ADDR_TORQUE_ENABLE = 64 # Control table address is different in Dynamixel model
ADDR_GOAL_POSITION = 116
ADDR_PRESENT_POSITION = 132
# Protocol version
PROTOCOL_VERSION = 2.0 # See which protocol version is used in the Dynamixel
# Default setting
DXL_ID = [11, 12, 13, 14, 15] # Dynamixel ID : 11~15
BAUDRATE = 57600 # Dynamixel default baudrate : 57600
DEVICENAME = '/dev/ttyUSB0' # Check which port is being used on your controller
# ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*"
TORQUE_ENABLE = 1 # Value for enabling the torque
TORQUE_DISABLE = 0 # Value for disabling the torque
DXL_MINIMUM_POSITION_VALUE = 0 # Dynamixel will rotate between this value
DXL_MAXIMUM_POSITION_VALUE = 1000 # 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)
def set_goal_pos_callback(data):
print("Set Goal Position of ID %s = %s" % (data.id, data.position))
dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, data.id, ADDR_GOAL_POSITION, data.position)
def get_present_pos(req):
dxl_present_position, dxl_comm_result, dxl_error = packetHandler.read4ByteTxRx(portHandler, req.id, ADDR_PRESENT_POSITION)
print("Present Position of ID %s = %s" % (req.id, dxl_present_position))
return dxl_present_position
def read_write_py_node():
rospy.init_node('read_write_py_node')
rospy.Subscriber('set_position', SetPosition, set_goal_pos_callback)
rospy.Service('get_position', GetPosition, get_present_pos)
rospy.spin()
def main():
# Open port
try:
portHandler.openPort()
print("Succeeded to open the port")
except:
print("Failed to open the port")
print("Press any key to terminate...")
getch()
quit()
# Set port baudrate
try:
portHandler.setBaudRate(BAUDRATE)
print("Succeeded to change the baudrate")
except:
print("Failed to change the baudrate")
print("Press any key to terminate...")
getch()
quit()
# Enable Dynamixel Torque
for id in DXL_ID:
dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, id, ADDR_TORQUE_ENABLE, TORQUE_ENABLE)
if dxl_comm_result != COMM_SUCCESS:
print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
print("Press any key to terminate...")
getch()
quit()
elif dxl_error != 0:
print("%s" % packetHandler.getRxPacketError(dxl_error))
print("Press any key to terminate...")
getch()
quit()
else:
print("DYNAMIXEL (ID; "+str(id)+") connected!")
print("Ready to get & set Position.")
read_write_py_node()
if __name__ == '__main__':
main()
**具体的な変更部分（3か所）**
path
# !/usr/bin/env python3
リスト化
# Default setting
DXL_ID = [11, 12, 13, 14, 15] # Dynamixel ID : 11~15
繰り返し処理へ
# Enable Dynamixel Torque
for id in DXL_ID:
作業②：joy_to_dynamixelのプログラム作成
方法としては，joy_to_twistのときと同様である．作成したプログラムは次に示す．なお，プログラムの名前は，モータを5つ動かすということで，簡単にmotor5.pyという名前にしている．
**joy_to_dynamixelプログラム**
motor5.py
# ! /usr/bin/env python3
"""
2021/06/05
@Yuya Shimizu
joy_to_dynamixel for 5 motors
"""
import rospy
from sensor_msgs.msg import Joy
from dynamixel_sdk_examples.msg import SetPosition
p = [2045, 2045, 2045, 2045, 2100]
def control(msg, dynamixel_pub):#joy to dynamixel
L_horizontal = msg.axes[0] #左ジョイスティック（左右）
L_vertical = msg.axes[1] #左ジョイスティック（上下）
R_horizontal = msg.axes[2] #左ジョイスティック（左右）
R_vertical = msg.axes[3] #左ジョイスティック（上下）
L2 = msg.axes[12] #L2
R2 = msg.axes[13] #R2
Input = [L_horizontal, L_vertical, R_horizontal, R_vertical, L2, R2]
#SetPositionのインスタンスを生成
d1 = SetPosition()
d2 = SetPosition()
d3 = SetPosition()
d4 = SetPosition()
d5 = SetPosition()
#IDの設定
d1.id = 11
d2.id = 12
d3.id = 13
d4.id = 14
d5.id = 15
#入力値の処理
for i in range(4):
if (p[i] >= 1022 and p[i] <= 3068):
p[i] = p[i] + 50 * Input[i]
if (p[i] > 3068):
p[i] = 3068
elif (p[i] < 1022):
p[i] = 1022
if (p[4] >= 1500):
p[4] = p[4] + 30 * L2
if (p[4] < 1500):
p[4] = 1500
if (p[0] <= 2700):
p[4] = p[4] - 30 * R2
if (p[4] > 2700):
p[4] = 2700
#positionの設定;dynamixelにjoyから取得したデータを当てはめる
d1.position = int(p[0])
d2.position = int(p[1])
d3.position = int(p[2])
d4.position = int(p[3])
d5.position = int(p[4])
#dynamixel positionを配信
dynamixel_pub.publish(d1)
dynamixel_pub.publish(d2)
dynamixel_pub.publish(d3)
dynamixel_pub.publish(d4)
dynamixel_pub.publish(d5)
if __name__ == '__main__':
#ノードの初期化
rospy.init_node('joy_to_dynamixel')
#配信準備
dynamixel_pub = rospy.Publisher('/set_position', SetPosition, queue_size=1)
#購読
rospy.Subscriber('/joy', Joy, control, dynamixel_pub)
rospy.spin()
このプログラムはdynamixel_sdk_examplesで用意されているmsg（SetPosition.msg）を用いたもので，少し不格好なプログラムかもしれないが，操作することは可能になる．
このときのSetPosition.msgを示しておく．
**SetPosition.msg**
SetPosition.msg
uint8 id
int32 position
作業③：launchファイルの作成
/joyを配信するps4joy.launchとdynamixelと通信するためのプログラムread_write_armbot.pyとjoy_to_dynamixelを担うプログラムmotor5.pyをまとめるlaunchファイルを作成した．
**dynamixel_motor5.launch**
dynamixel_motor5.launch
<launch>
<include file="$(find joy)/launch/ps4joy.launch"/>
<node name="connect" pkg="dynamixel_lesson" type="read_write_armbot.py" output="screen"/>
<node name="control" pkg="dynamixel_lesson" type="motor5.py" output="screen"/>
</launch>
実装（改善前）
先ほどまでの作業で動かす準備ができた．以下に実装手順とロボットの様子をそれぞれ動画で示す．
手順
ロボットの様子
動きはぎこちない部分が残る．特に右ジョイスティックの入力に対するID13, 14の反応が鈍すぎる．この原因は，おそらく第14弾の安定でない方を参考にjoy_to_dynamixelのプログラムを組んでしまっていたからであると考えられる．実際にターミナル（端末）で流れる情報を見ると，ID13, 14が欠落している．そこで，安定周期を導入したプログラムを組み込もうと思う．https://qiita.com/Yuya-Shimizu/items/bf58f6fa0925b5460022
作業②の改善
joy_to_dynamixelを安定周期を考慮せずに作っているため，コマンドラインの様子を見てもデータが一部飛んでいることが確認できる．第14弾のときにはコマンドの様子しか確認できなかったが，実際にロボットに適用するとこのように明らかなデータの欠損が動きに影響が出てくることを体感した．そこで，次のように改善してみた．
改善の方針としては，先ほどのmotor5.pyを改造することである．改善にあたって，今回は5つのSetPositionインスタンスを用いているため，次のようなプログラムとした．
**motor5_improved.py**
motor5_improved.py
# ! /usr/bin/env python3
"""
2021/06/05
@Yuya Shimizu
joy_to_dynamixel for 5 motors (improved)
"""
import rospy
from sensor_msgs.msg import Joy
from dynamixel_sdk_examples.msg import SetPosition
p = [2045, 2045, 2045, 2045, 2100]
def control(msg, dynamixel_pub):#joy to dynamixel
global d1, d2, d3, d4, d5
L_horizontal = msg.axes[0] #左ジョイスティック（左右）
L_vertical = msg.axes[1] #左ジョイスティック（上下）
R_horizontal = msg.axes[2] #左ジョイスティック（左右）
R_vertical = msg.axes[3] #左ジョイスティック（上下）
L2 = msg.axes[12] #L2
R2 = msg.axes[13] #R2
Input = [L_horizontal, L_vertical, R_horizontal, R_vertical, L2, R2]
#SetPositionのインスタンスを生成
d1 = SetPosition()
d2 = SetPosition()
d3 = SetPosition()
d4 = SetPosition()
d5 = SetPosition()
#IDの設定
d1.id = 11
d2.id = 12
d3.id = 13
d4.id = 14
d5.id = 15
#入力値の処理
for i in range(4):
if (p[i] >= 1022 and p[i] <= 3068):
p[i] = p[i] + 50 * Input[i]
if (p[i] > 3068):
p[i] = 3068
elif (p[i] < 1022):
p[i] = 1022
if (p[4] >= 1500):
p[4] = p[4] + 30 * L2
if (p[4] < 1500):
p[4] = 1500
if (p[0] <= 2700):
p[4] = p[4] - 30 * R2
if (p[4] > 2700):
p[4] = 2700
#positionの設定;dynamixelにjoyから取得したデータを当てはめる
d1.position = int(p[0])
d2.position = int(p[1])
d3.position = int(p[2])
d4.position = int(p[3])
d5.position = int(p[4])
#dynamixel positionを配信
dynamixel_pub.publish(d1)
dynamixel_pub.publish(d2)
dynamixel_pub.publish(d3)
dynamixel_pub.publish(d4)
dynamixel_pub.publish(d5)
if __name__ == '__main__':
#ノードの初期化
rospy.init_node('joy_to_dynamixel')
#配信準備
dynamixel_pub = rospy.Publisher('/set_position', SetPosition, queue_size=1)
#購読
rospy.Subscriber('/joy', Joy, control, dynamixel_pub)
rate = rospy.Rate(10) #10Hz(100ミリ秒ごと)で出力するため
d1 = SetPosition() #0に初期化
d2 = SetPosition()
d3 = SetPosition()
d4 = SetPosition()
d5 = SetPosition()
while not rospy.is_shutdown():
dynamixel_pub.publish(d1)
rate.sleep()
dynamixel_pub.publish(d2)
rate.sleep()
dynamixel_pub.publish(d3)
rate.sleep()
dynamixel_pub.publish(d4)
rate.sleep()
dynamixel_pub.publish(d5)
rate.sleep()
使うプログラムが変わったので，launchファイルも編集しておく．
**dynamixel_motor5.launch**
dynamixel_motor5.launch
<launch>
<include file="$(find joy)/launch/ps4joy.launch"/>
<node name="connect" pkg="dynamixel_lesson" type="read_write_armbot.py" output="screen"/>
<node name="control" pkg="dynamixel_lesson" type="motor5_improved.py" output="screen"/>
</launch>
改善作業は以上である．
実装（改善）
先ほどの改善を踏まえて，以下に実装手順とロボットの様子をそれぞれ動画で示す．
手順
ロボットの様子
先ほどよりかかなり改善された．特にID13, 14のところがうまく入力に応答できていることが確認できる．先ほどまでは，うまく配信できていなかったようだ．やはり第14弾の安定でない方を参考にjoy_to_dynamixelのプログラムを組んでしまっていたために，一部が欠落してしまっていたことが原因であった．
感想
ついにDynamixelをコントローラで操作できるようになった．ラジコンを作っている気分で非常に面白い．過去に投稿した第14弾の内容の理解も深められたと思う．第42弾で，つい先走って安定でないkey_to_twistを参考にしてしまっていた．その時はそこまで速いデータのやり取りはなく，データ数も少なかったため，特に問題がなかったと思われる．おかげで，良い勉強となったとは思う．今まで適当にrate.sleep()やqueue_sizeを扱ってきたが，ここでそれらの意味するところを体験することができた．機会があれば，コントローラではなく，MoveItを使った制御も試してみたいと思っている．やはり，実際にロボットが動くとさらに楽しい．