はじめに

こんにちは。しがない高校生をしているbasalte(ばさると)と申します。

今回はROS2 HumbleでOpenmanipulatorを動かすまでに起こった問題等をメモ代わりに残しておきます。どなたかの役に立てば幸いです。

環境

・Ubuntu 22.04
・ROS2 Humble
使用マイコンはOpenCRです。
Openmanipulator-x,OpenCRはどちらもROBOTISからお貸しいただいています。本当にありがとうございます。

環境構築

OpenCRを接続

基本的にROBOTISのe-Manualに従います。
https://emanual.robotis.com/docs/en/parts/controller/opencr10_jp/

OpenCRのUSBポートがroot権限無しでArduinoスケッチをアップできるようにします。

$ wget https://raw.githubusercontent.com/ROBOTIS-GIT/OpenCR/master/99-opencr-cdc.rules
$ sudo cp ./99-opencr-cdc.rules /etc/udev/rules.d/
$ sudo udevadm control --reload-rules
$ sudo udevadm trigger

次にArduino IDEに合わせた32bitコンパイラを用意します。

$ sudo apt-get install libncurses5-dev:i386

次にArduino IDEをインストールします。公式サイトにアクセスし、IDE 2の新しいバージョンを選択します。
Arduino IDE 1.8.Xの古い方だとこの後設定できないので気をつけて下さい。
どこかのフォルダに解凍したら下で起動します。

$ arduino-ide

Arduino IDEの環境設定

Arduino IDEを開いたら左上の「ファイル」>「基本設定」を開いて、
「追加のボードマネジャーのURL」に以下のURLを貼り付けます。

https://raw.githubusercontent.com/ROBOTIS-GIT/OpenCR/master/arduino/opencr_release/package_opencr_index.json

次に「ツール」>「ボード」>「ボードマネージャー」を開いて、
出てきた検索窓にOpenCRと打ちます。するとROBOTIS製のパッケージが出てくるはずなのでこれをインストールします。そこそこ時間かかります。

インストールが完了したら「ツール」>「ボード」からOpenCRを選択できるようになっているはずです。

OpenCRを選択すると、「ファイル」>「スケッチ例」からOpenCR、Openmanipulatorやその他もろもろのサンプルコードを使うことができます。

自分は当たったことがない問題なのですが、書き込みがうまく行かないときは下を実行すると治ることがあるらしいです。

$ sudo apt-get purge modemmanager

ROSで通信する準備

次にROSで通信をするので、usbから通信を受け取るためのプログラムを書き込みます。
「ファイル」>「スケッチ例」>「OpenCR」>「Etc」>「usb_to_dxl」を開いて、書き込みます。

Arduino IDE側の環境構築は終わりです。

書き込めない、usbを認識しない場合

OpenCR側が調子が悪いときは、以下のことを実行します。

1.PUSH SW2をずっと押しておく
2.RESETを長押しする
3.RESETを離す
4.PUSH SW2を離す

をするとリカバリーモードが起動し、ステータスLEDが一定周期で点滅します。この状態になると絶対にusbで認識できるようになります。
この状態でArduino IDEでボードとポートを選択し、下の初期プログラムを書き込みます。

void setup() {
  // put your setup code here, to run once:

}

void loop() {
  // put your main code here, to run repeatedly:

}

これで正常に書き込めるようになるはずです。

Dynamixelが点滅している

OpenCRから何かしらの司令を送っているけど動かず、DynamixelのLEDが点滅している場合、何かしらのエラーが出ています。

まずやることはDynamixelの配線の確認です。最初のころはしっかり差し込めていないことが多いです。力の限りグッと押し込みましょう。

それでも治らない場合、点滅しているDynamixelを分解します。

このモーターにつながっている配線が外れていることがあるので、付け直してもう一度動かしてみて下さい。自分はこれに遭遇し、はんだ付けしようと思っています。

Dynamixelを一個しか認識しない

これはDynamixel wizerdですべてのDynamixelをファームウェアアップデートした時に起こりました。
一個だけならしっかり認識してトルクも効くのに、二個目から全く認識できません。
これは、DynamixelがIDをそれぞれ振られていて、そのIDに対して制御コマンドが送られることに由来しています。

デフォルトのOpenmanipulatorはIDが11~15に割り振られていますが、ファームウェアアップデートをするとすべてのIDが1になります。
この状態で複数つなぐと、当然IDがかぶってしまいます。

解決方法は一つずつDynamixelをつないでそれぞれのIDを変えてあげることです。wizerdで実行することができます。

ROS2 Humbleを動かす

ROS2 HumbleでOpenmanipulator-xを動かしていきます。
先に以下のコマンドを打ちます

$ sudo apt install ros-humble-rqt* ros-humble-joint-state-publisher

次にワークスペースを作成し、色々とgit cloneしていきます。

$ cd ~/colcon_ws/src/
$ git clone -b humble-devel https://github.com/ROBOTIS-GIT/DynamixelSDK.git
$ git clone -b ros2 https://github.com/ROBOTIS-GIT/dynamixel-workbench.git
$ git clone -b humble-devel https://github.com/ROBOTIS-GIT/open_manipulator.git
$ git clone -b ros2 https://github.com/ROBOTIS-GIT/open_manipulator_msgs.git
$ git clone -b ros2 https://github.com/ROBOTIS-GIT/open_manipulator_dependencies.git
$ git clone -b ros2 https://github.com/ROBOTIS-GIT/robotis_manipulator.git
$ cd ~/colcon_ws && colcon build --symlink-install

そうしたらROS2 で動かしていきます。

$ cd ~/colcon_ws
$ source /opt/ros/humble/setup.bash
$ source install/local_setup.bash
$ ros2 launch open_manipulator_x_controller open_manipulator_x_controller.launch.py usb_port:=/dev/ttyACM0

これでOpenCRと接続し、Openmanipulator-xが位置制御に入った(今の位置を維持しようと踏ん張った)ら成功です。
process has diedというログが出たら、下のOpenCRと接続できないを試してみて下さい。

成功したら、下の２つのコマンドを別のターミナルで打って、キーボードで遊びましょう。

rviz2とkeyboard_teleopを起動する

$ ros2 launch open_manipulator_x_description open_manipulator_x_rviz.launch.py 
$ ros2 run open_manipulator_x_teleop teleop_keyboard

OpenCRと接続できない

上のlaunchファイルを実行するとprocess has diedとなり終了してしまう場合、いくつか原因があります。

OpenCRにusb_to_dxlを書き込んでいない

ROSで通信する準備に書いてあるとおり、ROSを実行する前に書き込まないといけないプログラムがあるので、こちらをまずは実行して下さい。

Dynamixelに接続するbpsが間違っている

$ ros2 launch open_manipulator_x_controller open_manipulator_x_controller.launch.py usb_port:=/dev/ttyACM0

このコマンドでポートを正しく選択していて、実行直後は一瞬だけOpenCRが点滅するという方は、私と同じミスをしています。
OpenCRにusbで接続をするのと同時に、Dynamixelにも接続するオプションが存在します。

ワークスペースを開いて、

src/openmanipulator/openmanipulator_x_controller/launch/open_manipulator_x_controller.launch.py

これを開いて、以下の部分を変更します。

def generate_launch_description():
    # Parameters
    usb_port = LaunchConfiguration('usb_port', default='/dev/ttyUSB0')
    baud_rate = LaunchConfiguration('baud_rate', default='1000000')
    param_dir = LaunchConfiguration(
        'param_dir',
        default=os.path.join(
            get_package_share_directory('open_manipulator_x_controller'),
            'param',
            'open_manipulator_x_controller_params.yaml'))

'usb_port'はオプションで変更していましたが、'band_rate'もDynamixelに合わせないと実行することができません。
wizerdを使ってDynamixelが使用している通信レートを調べます。自分は57600だったので、以下のように変更します。

('usb_port', default='/dev/ttyACM0')
('baud_rate', default='57600')

修正したら、もう一度オプションなしで先程のコマンドを実行してみましょう。
--symlink-installをつけてビルドしていれば再ビルドしなくてもpythonファイルであれば読み込まれます。
これが成功したら、先程の場所に戻ってrviz2と
keyboard_teleopを実行しましょう。

keyboard_teleopでGripperが動かない

どれだけgとfを押してもGripperが動いてくれないという方、その原因はOpenCRでもDynamixelでもROS2でもなくGripperを動かすプログラムが存在しないことかもしれません。

自分は適当にROS Noeticとかの実装を読んで以下のコードを追加しました。

/src/open_manipulator/open_manipulator_x_teleop/script/teleop_keyboard.py

このpythonファイルに

5つめのリスト要素を追加し、

present_joint_angle = [0.0, 0.0, 0.0, 0.0 ,0.0]
goal_joint_angle = [0.0, 0.0, 0.0, 0.0 ,0.0]
prev_goal_joint_angle = [0.0, 0.0, 0.0, 0.0 ,0.0]

tool_controlとtool_control_reqを追加しSetJointPositionし、

self.goal_joint_space = self.create_client(SetJointPosition, 'goal_joint_space_path')
self.goal_task_space = self.create_client(SetKinematicsPose, 'goal_task_space_path')
self.tool_control = self.create_client(SetJointPosition, 'goal_tool_control')
self.goal_joint_space_req = SetJointPosition.Request()
self.goal_task_space_req = SetKinematicsPose.Request()
self.tool_control_req = SetJointPosition.Request()

jointのリストに'Gripper'を追加し、

def send_goal_joint_space(self):
    self.goal_joint_space_req.joint_position.joint_name = ['joint1', 'joint2', 'joint3', 'joint4' ,'gripper']

callbackやformatにmsg.position[4]を追加してあげて

def joint_state_callback(self, msg):
    present_joint_angle[0] = msg.position[0]
    present_joint_angle[1] = msg.position[1]
    present_joint_angle[2] = msg.position[2]
    present_joint_angle[3] = msg.position[3]
    present_joint_angle[4] = msg.position[4]

def print_present_values():
    print(usage)
    print('Joint Angle(Rad): [{:.6f}, {:.6f}, {:.6f}, {:.6f} ,{:.6f}]'.format(
        present_joint_angle[0],
        present_joint_angle[1],
        present_joint_angle[2],
        present_joint_angle[3],
        present_joint_angle[4]))

キーボード入力の条件分岐を追加してあげれば一通り動くようになるはずです

elif key_value == 'f':
    goal_joint_angle[4] = prev_goal_joint_angle[4] + 0.01
    teleop_keyboard.send_tool_control_request()
elif key_value == 'g':
    goal_joint_angle[4] = prev_goal_joint_angle[4] - 0.01
    teleop_keyboard.send_tool_control_request()

なぜ公式配布にこのコードが存在しないのかはよくわかりませんが、プログラムの理解につながったので結果的に良かったです。

一通り実行できた
手で押さえてるのはご容赦 pic.twitter.com/SQ2J4qfdyT
— basalte(ばざると) (@basalte1199) January 6, 2024

以上で一通りOpenmanipulator-xを動かすことができるようになると思います。
不備などあったらコメントなどいただけると幸いです。