ROSでDynamixelを動かす方法１ - 自分用知識置き場

この記事は、ROS1及び通信プロトコル2.0のDynamixel xシリーズを使用しています。

環境
ROSのインストール
Dynamixelの環境構築
Dynamixelのパッケージの作成
モータの配線と各種設定
接続確認
Topic通信で動かす
Service通信で動かす

環境

Ubuntu20.04
ROS Noetic

ROSのインストール

公式のインストール手順

sources.list をセットアップする

sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'

キーの設定

 sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654

パッケージを更新

sudo apt update

インストール

sudo apt install ros-noetic-desktop-full

環境設定

echo "source /opt/ros/noetic/setup.bash" >> ~/.bashrc

source ~/.bashrc

パッケージを構築するための依存関係

sudo apt install python3-rosdep python3-rosinstall python3-rosinstall-generator python3-wstool build-essential

rosdepの初期化

sudo rosdep init

rosdep update

catkin buildを使えるようにする

sudo apt install python3-osrf-pycommon python3-catkin-tools

sudo apt install build-essential

ROSワークスペースの作成

mkdir -p ~/catkin_ws/src

cd ~/catkin_ws/

catkin build

echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc

source ~/.bashrc

Dynamixelの環境構築

公式の環境構築手順

DYNAMIXEL Workbench

ライブラリのダウンロード

cd ~/catkin_ws/src

git clone https://github.com/ROBOTIS-GIT/dynamixel-workbench.git

git clone https://github.com/ROBOTIS-GIT/dynamixel-workbench-msgs.git

git clone https://github.com/ROBOTIS-GIT/DynamixelSDK.git

catkin build

rulesファイルのコピー

cd ~/

wget https://raw.githubusercontent.com/ROBOTIS-GIT/dynamixel-workbench/master/99-dynamixel-workbench-cdc.rules

sudo cp ./99-dynamixel-workbench-cdc.rules /etc/udev/rules.d/

sudo udevadm control --reload-rules

sudo udevadm trigger

Dynamixelのパッケージの作成

今回は、チュートリアルのパッケージを作ります。

パッケージの作成

cd ~/catkin_ws/src

catkin_create_pkg dynamixel_tutorial rospy

mkdir -p dynamixel_tutorial/{config,launch}

cp dynamixel-workbench/dynamixel_workbench_controllers/launch/dynamixel_controllers.launch dynamixel_tutorial/launch/

cp dynamixel-workbench/dynamixel_workbench_controllers/config/joint_2_0.yaml dynamixel_tutorial/config/

dynamixel_tutorialに複製したconfigファイルをlaunchで読み込めるようにするため、dynamixel_controllers.launchを編集

cd ~/catkin_ws/src/dynamixel_tutorial/launch

vim dynamixel_controllers.launch

10行目のvalueを下記に変更

<param name="dynamixel_info"          value="$(find dynamixel_tutorial)/config/joint_2_0.yaml"/>

vimを終了させて、ビルド

catkin build

モータの配線と各種設定

下の記事に詳しく記載されているので、この記事では省略します。

www.besttechnology.co.jp

主にやることは次の通りです。

配線
DYNAMIXEL Wizard 2.0のインストール
DYNAMIXEL Wizard 2.0でID番号を設定する

今回は、Dynamixel xシリーズのモータを２個使用します。

接続確認

sudo chmod 666 /dev/ttyUSB0

roscore

rosrun dynamixel_workbench_controllers find_dynamixel /dev/ttyUSB0

id : 1とid : 2のモータが出力されたことを確認する。

Topic通信で動かす

この例では、以下のように動作させます。

ID:1のモータを1.0秒かけて30°に回転
ID:1とID:2のモータを1.0秒かけて-30°に回転
ID:1とID:2のモータを1.5秒かけて0°に回転
Pythonファイルの作成

cd ~/catkin_ws/src/dynamixel_tutorial/src

vim by_topic.py

以下のコードを張り付ける

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

import math
import rospy
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint

def degToRad(deg):
    return math.radians(deg)

def motorPub(joint_name, joint_angle, execute_time=1.0):
    # Publisherの定義
    motor_pub = rospy.Publisher('/dynamixel_workbench/joint_trajectory',JointTrajectory,queue_size=10)
    rospy.sleep(0.5)
    
    # publishするデータの定義
    msg = JointTrajectory()
    msg.header.stamp = rospy.Time.now()
    msg.joint_names = joint_name
    msg.points = [JointTrajectoryPoint()]
    msg.points[0].positions = list(map(degToRad, joint_angle))
    msg.points[0].time_from_start = rospy.Time(execute_time)
    # publish
    motor_pub.publish(msg)

if __name__ == '__main__':
    rospy.init_node('dynamixel_by_topic')
    motorPub(['pan'], [30])
    rospy.sleep(2.0)
    motorPub(['pan', 'tilt'], [-30, -30])
    rospy.sleep(2.0)
    motorPub(['pan', 'tilt'], [0, 0], 1.5)

pythonファイルに権限を付与させる

chmod 755 by_topic.py

実行させる

roslaunch dynamixel_tutorial dynamixel_controllers.launch

rosrun dynamixel_tutorial by_topic.py

Service通信で動かす

Pythonファイルの作成

cd ~/catkin_ws/src/dynamixel_tutorial/src

vim by_service.py

以下のコードを張り付ける

# Service制御
def degToStep(deg):
    return int((deg+180)/360.0*4095)

def setPosition(motor_id, position):
    # Service Clientの定義
    motor_client = rospy.ServiceProxy('/dynamixel_workbench/dynamixel_command',DynamixelCommand)
    
    # degreesを4096段階のstep(12bit)に変換
    step = degToStep(position)
    
    # 許容電圧を指定
    motor_client('', motor_id, 'Goal_Current', 200)
    # 目標角度を指定
    motor_client('', motor_id, 'Goal_Position', step)
    
if __name__ == '__main__':
    rospy.init_node('dynamixel_by_service')
    setPosition(0, 30)
    rospy.sleep(2.0)
    setPosition(1, 30)
    rospy.sleep(2.0)
    setPosition(0, 0)
    setPosition(1, 0)

pythonファイルに権限を付与させる

chmod 755 by_topic.py

実行させる

roslaunch dynamixel_tutorial dynamixel_controllers.launch

rosrun dynamixel_tutorial by_service.py